########################################################################
#
# Copyright (c) 2022, STEREOLABS.
#
# All rights reserved.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
########################################################################

import os
import sys
import ctypes
import cv2
import numpy as np
import threading
import time
import json
import warnings

# 过滤 PyQt5 弃用警告
warnings.filterwarnings("ignore", category=DeprecationWarning, module="PyQt5")

# 导入matplotlib相关模块
try:
    import matplotlib.pyplot as plt
    from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
    from matplotlib.figure import Figure
    import matplotlib.dates as mdates
    MATPLOTLIB_AVAILABLE = True
except ImportError:
    MATPLOTLIB_AVAILABLE = False
    print("警告: matplotlib未安装，将无法显示实时曲线图")

from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, 
                             QHBoxLayout, QLabel, QPushButton, QSlider, QGroupBox,
                             QGridLayout, QMessageBox, QCheckBox, QSpinBox, QLineEdit,
                             QScrollArea, QDialog, QSizePolicy, QComboBox)
from PyQt5.QtCore import QThread, pyqtSignal, Qt, QTimer, QObject
from PyQt5.QtGui import QPixmap, QImage, QMouseEvent

# 在导入 pyzed.sl 之前先加载必要的 DLL 文件
print("正在加载 ZED SDK DLL 文件...")

# 设置 DLL 搜索路径
dll_paths = [
    r"C:\Program Files (x86)\ZED SDK\bin",
    r"C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v12.9\bin"
]

# 将路径添加到 DLL 搜索路径
for path in dll_paths:
    if os.path.exists(path):
        os.add_dll_directory(path)
        print(f"  ✓ 添加 DLL 搜索路径: {path}")

# 预加载关键的 DLL 文件
try:
    print("预加载 sl_zed64.dll...")
    sl_zed = ctypes.CDLL(r"C:\Program Files (x86)\ZED SDK\bin\sl_zed64.dll")
    print("  ✓ sl_zed64.dll 加载成功")
except Exception as e:
    print(f"  ✗ sl_zed64.dll 加载失败: {e}")

try:
    print("预加载 sl_ai64.dll...")
    sl_ai = ctypes.CDLL(r"C:\Program Files (x86)\ZED SDK\bin\sl_ai64.dll")
    print("  ✓ sl_ai64.dll 加载成功")
except Exception as e:
    print(f"  ✗ sl_ai64.dll 加载失败: {e}")

print("DLL 加载完成，现在导入 pyzed.sl...")
import pyzed.sl as sl


class CoordinateSmoothingProcessor:
    """坐标滑动平均处理器"""
    
    def __init__(self, window_size=5):
        self.window_size = window_size
        self.coordinate_history = {
            'x': [],
            'y': [],
            'z': []
        }
        self.is_enabled = True
    
    def set_window_size(self, size):
        """设置滑动窗口大小"""
        self.window_size = max(2, min(20, size))
        # 清理历史数据，确保不超过新的窗口大小
        while len(self.coordinate_history['x']) > self.window_size:
            self.coordinate_history['x'].pop(0)
            self.coordinate_history['y'].pop(0)
            self.coordinate_history['z'].pop(0)
    
    def enable(self, enabled):
        """启用或禁用滑动平均"""
        self.is_enabled = enabled
        if not enabled:
            # 禁用时清空历史数据
            self.coordinate_history = {'x': [], 'y': [], 'z': []}
    
    def add_coordinate(self, x, y, z):
        """添加新的坐标数据"""
        if not self.is_enabled:
            return x, y, z
        
        # 添加新数据
        self.coordinate_history['x'].append(x)
        self.coordinate_history['y'].append(y)
        self.coordinate_history['z'].append(z)
        
        # 限制历史数据长度
        if len(self.coordinate_history['x']) > self.window_size:
            self.coordinate_history['x'].pop(0)
            self.coordinate_history['y'].pop(0)
            self.coordinate_history['z'].pop(0)
        
        # 计算滑动平均
        if len(self.coordinate_history['x']) >= 2:
            avg_x = sum(self.coordinate_history['x']) / len(self.coordinate_history['x'])
            avg_y = sum(self.coordinate_history['y']) / len(self.coordinate_history['y'])
            avg_z = sum(self.coordinate_history['z']) / len(self.coordinate_history['z'])
            return avg_x, avg_y, avg_z
        else:
            # 数据不足时返回原始值
            return x, y, z
    
    def reset(self):
        """重置滑动平均器"""
        self.coordinate_history = {'x': [], 'y': [], 'z': []}
    
    def get_status_info(self):
        """获取状态信息"""
        if not self.is_enabled:
            return "状态: 已禁用"
        else:
            current_count = len(self.coordinate_history['x'])
            return f"状态: 已启用 ({current_count}/{self.window_size}次平均)"


class DataCollectionThread(QThread):
    """数据采集线程 - 专门用于收集三维坐标数据，不进行复杂计算"""
    
    # 信号定义
    data_collected = pyqtSignal(float, float, float, float, float, float)  # 发送原始和平滑后的X, Y, Z坐标数据
    
    def __init__(self):
        super().__init__()
        self.is_running = False
        self.coordinate_queue = []  # 简单的数据队列
        self.max_queue_size = 50   # 限制队列大小
        self.mutex = threading.Lock()
        
    def add_coordinate_data(self, x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth):
        """添加原始和平滑后的坐标数据到队列"""
        with self.mutex:
            self.coordinate_queue.append((x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth))
            # 限制队列大小
            if len(self.coordinate_queue) > self.max_queue_size:
                self.coordinate_queue.pop(0)
    
    def run(self):
        """线程运行函数"""
        self.is_running = True
        
        while self.is_running:
            try:
                # 检查是否有新数据
                with self.mutex:
                    if self.coordinate_queue:
                        x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth = self.coordinate_queue.pop(0)
                        # 发送数据信号
                        self.data_collected.emit(x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth)
                
                # 休眠时间更长，减少CPU占用
                time.sleep(0.2)  # 200ms检查一次
                
            except Exception as e:
                print(f"数据采集线程错误: {e}")
                time.sleep(0.2)
    
    def stop(self):
        """停止线程"""
        self.is_running = False


class ImageDisplayThread(QThread):
    """图像显示线程 - 专门负责图像显示，不进行图像处理"""
    
    # 信号定义
    left_image_display_ready = pyqtSignal(object)  # 发送左图像用于显示
    right_image_display_ready = pyqtSignal(object)  # 发送右图像用于显示
    blob_detection_ready = pyqtSignal(list, list, list, list)  # 发送光斑检测结果
    
    def __init__(self):
        super().__init__()
        self.is_running = False
        self.image_queue = []  # 图像队列
        self.max_queue_size = 3  # 限制队列大小，避免延迟
        self.mutex = threading.Lock()
        
    def add_image_data(self, left_image, right_image, left_centroids, right_centroids, left_contours, right_contours):
        """添加图像数据到队列"""
        with self.mutex:
            # 限制队列大小，保持最新的图像
            if len(self.image_queue) >= self.max_queue_size:
                self.image_queue.pop(0)
            
            # 确保图像已经绘制了光斑检测结果
            if left_image is not None and right_image is not None:
                self.image_queue.append({
                    'left_image': left_image,
                    'right_image': right_image,
                    'left_centroids': left_centroids,
                    'right_centroids': right_centroids,
                    'left_contours': left_contours,
                    'right_contours': right_contours
                })
    
    def run(self):
        """线程运行函数"""
        self.is_running = True
        
        while self.is_running:
            try:
                # 检查是否有新图像数据
                with self.mutex:
                    if self.image_queue:
                        image_data = self.image_queue.pop(0)
                        
                        # 发送图像用于显示
                        self.left_image_display_ready.emit(image_data['left_image'])
                        self.right_image_display_ready.emit(image_data['right_image'])
                        
                        # 发送光斑检测结果
                        self.blob_detection_ready.emit(
                            image_data['left_centroids'],
                            image_data['right_centroids'],
                            image_data['left_contours'],
                            image_data['right_contours']
                        )
                
                # 控制显示频率，与图像捕获同步
                time.sleep(0.033)  # 约30FPS
                
            except Exception as e:
                print(f"图像显示线程错误: {e}")
                time.sleep(0.033)
    
    def stop(self):
        """停止线程"""
        self.is_running = False


class RealTimePlotThread(QThread):
    """实时曲线图显示线程 - 专门处理图表显示，不进行数据计算"""
    
    # 信号定义
    plot_update_ready = pyqtSignal(object)  # 发送图表数据用于更新显示
    
    def __init__(self):
        super().__init__()
        self.is_running = False
        self.coordinate_history = {
            'timestamps': [],
            'raw_x_coords': [],
            'raw_y_coords': [],
            'raw_z_coords': [],
            'smooth_x_coords': [],
            'smooth_y_coords': [],
            'smooth_z_coords': []
        }
        self.max_history_points = 20  # 减少历史数据点数量，提高性能
        self.mutex = threading.Lock()
        
    def add_coordinate_data(self, x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth):
        """添加原始和平滑后的坐标数据（从数据采集线程接收）"""
        with self.mutex:
            current_time = time.time()
            
            # 添加新数据
            self.coordinate_history['timestamps'].append(current_time)
            self.coordinate_history['raw_x_coords'].append(x_raw)
            self.coordinate_history['raw_y_coords'].append(y_raw)
            self.coordinate_history['raw_z_coords'].append(z_raw)
            self.coordinate_history['smooth_x_coords'].append(x_smooth)
            self.coordinate_history['smooth_y_coords'].append(y_smooth)
            self.coordinate_history['smooth_z_coords'].append(z_smooth)
            
            # 限制历史数据点数量
            if len(self.coordinate_history['timestamps']) > self.max_history_points:
                self.coordinate_history['timestamps'].pop(0)
                self.coordinate_history['raw_x_coords'].pop(0)
                self.coordinate_history['raw_y_coords'].pop(0)
                self.coordinate_history['raw_z_coords'].pop(0)
                self.coordinate_history['smooth_x_coords'].pop(0)
                self.coordinate_history['smooth_y_coords'].pop(0)
                self.coordinate_history['smooth_z_coords'].pop(0)
    
    def run(self):
        """线程运行函数"""
        self.is_running = True
        
        while self.is_running:
            try:
                # 发送当前数据用于更新图表
                with self.mutex:
                    data_copy = {
                        'timestamps': self.coordinate_history['timestamps'].copy(),
                        'raw_x_coords': self.coordinate_history['raw_x_coords'].copy(),
                        'raw_y_coords': self.coordinate_history['raw_y_coords'].copy(),
                        'raw_z_coords': self.coordinate_history['raw_z_coords'].copy(),
                        'smooth_x_coords': self.coordinate_history['smooth_x_coords'].copy(),
                        'smooth_y_coords': self.coordinate_history['smooth_y_coords'].copy(),
                        'smooth_z_coords': self.coordinate_history['smooth_z_coords'].copy()
                    }
                
                self.plot_update_ready.emit(data_copy)
                
                # 降低更新频率，减少CPU占用
                time.sleep(0.5)  # 500ms更新一次
                
            except Exception as e:
                print(f"实时曲线图线程错误: {e}")
                time.sleep(0.5)
    
    def stop(self):
        """停止线程"""
        self.is_running = False


class BlobDetector:
    """光斑检测器 - 使用轮廓法"""
    
    def __init__(self):
        self.min_area = 1  # 最小面积要求，检测更小的光斑
        self.max_area = 20000  # 增加最大面积范围
        self.min_circularity = 0.6  # 圆形度要求（当前未使用）
        self.min_convexity = 0.7  # 凸度要求（当前未使用）
        self.threshold_value = 150  # 降低二值化阈值，更容易检测到光斑
        
        # 光斑稳定性增强参数
        self.blob_history = {}  # 存储光斑历史位置
        self.history_window = 3  # 减少历史窗口大小，提高响应性
        self.position_threshold = 15  # 增加位置变化阈值
        self.stability_threshold = 2  # 降低稳定性阈值，更快检测到光斑
    
    def detect_blobs(self, image):
        """使用轮廓法检测光斑（简化版本，直接面积约束）"""
        try:
            # 转换为灰度图
            if len(image.shape) == 3:
                gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
            else:
                gray = image
            
            # 直接使用单一阈值，不进行预处理
            _, thresh = cv2.threshold(gray, self.threshold_value, 255, cv2.THRESH_BINARY)
            
            # 查找轮廓
            contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            # 只进行面积过滤，不进行其他形状验证
            valid_contours = []
            
            for contour in contours:
                area = cv2.contourArea(contour)
                
                # 只检查面积约束
                if area >= self.min_area and area <= self.max_area:
                    valid_contours.append(contour)
            
            return valid_contours, thresh
            
        except Exception as e:
            return [], None
    
    def enhance_blob_stability(self, current_blobs):
        """增强光斑稳定性，减少闪烁"""
        try:
            stable_blobs = []
            current_time = time.time()
            
            for cx, cy, area, contour in current_blobs:
                # 为每个光斑创建唯一标识
                blob_id = self.find_matching_blob_id(cx, cy, area)
                
                if blob_id is not None:
                    # 更新现有光斑的历史记录
                    if blob_id in self.blob_history:
                        history = self.blob_history[blob_id]
                        history['positions'].append((cx, cy))
                        history['areas'].append(area)
                        history['timestamps'].append(current_time)
                        history['detection_count'] += 1
                        
                        # 保持历史窗口大小
                        if len(history['positions']) > self.history_window:
                            history['positions'].pop(0)
                            history['areas'].pop(0)
                            history['timestamps'].pop(0)
                        
                        # 检查稳定性
                        if history['detection_count'] >= self.stability_threshold:
                            # 计算平均位置和面积
                            avg_x = sum(pos[0] for pos in history['positions']) / len(history['positions'])
                            avg_y = sum(pos[1] for pos in history['positions']) / len(history['positions'])
                            avg_area = sum(history['areas']) / len(history['areas'])
                            
                            # 使用平均位置创建稳定的光斑
                            stable_blobs.append((avg_x, avg_y, avg_area, contour))
                    else:
                        # 新光斑，初始化历史记录
                        self.blob_history[blob_id] = {
                            'positions': [(cx, cy)],
                            'areas': [area],
                            'timestamps': [current_time],
                            'detection_count': 1
                        }
                else:
                    # 无法匹配的光斑，直接添加（可能是新出现的光斑）
                    stable_blobs.append((cx, cy, area, contour))
            
            # 清理过期的光斑历史记录
            self.cleanup_old_blobs(current_time)
            
            return stable_blobs
            
        except Exception as e:
            print(f"光斑稳定性增强失败: {e}")
            return current_blobs
    
    def find_matching_blob_id(self, cx, cy, area):
        """查找匹配的光斑ID"""
        try:
            for blob_id, history in self.blob_history.items():
                if len(history['positions']) > 0:
                    # 获取最近的位置
                    last_x, last_y = history['positions'][-1]
                    
                    # 计算位置距离
                    distance = np.sqrt((cx - last_x)**2 + (cy - last_y)**2)
                    
                    # 计算面积变化比例
                    if len(history['areas']) > 0:
                        last_area = history['areas'][-1]
                        area_ratio = abs(area - last_area) / max(area, last_area, 1)
                    else:
                        area_ratio = 0
                    
                    # 如果位置和面积都匹配，返回ID
                    if distance <= self.position_threshold and area_ratio <= 0.5:
                        return blob_id
            
            # 没有找到匹配，生成新ID
            return len(self.blob_history)
            
        except Exception as e:
            print(f"查找光斑ID失败: {e}")
            return None
    
    def cleanup_old_blobs(self, current_time):
        """清理过期的光斑历史记录"""
        try:
            expired_ids = []
            for blob_id, history in self.blob_history.items():
                if len(history['timestamps']) > 0:
                    # 如果超过2秒没有检测到，标记为过期
                    time_since_last = current_time - history['timestamps'][-1]
                    if time_since_last > 2.0:
                        expired_ids.append(blob_id)
            
            # 删除过期的记录
            for blob_id in expired_ids:
                del self.blob_history[blob_id]
                
        except Exception as e:
            print(f"清理过期光斑记录失败: {e}")
    
    def is_circular_blob(self, contour, area):
        """验证光斑是否为圆形"""
        try:
            # 计算轮廓的边界框
            x, y, w, h = cv2.boundingRect(contour)
            
            # 检查宽高比（圆形光斑的宽高比应该接近1）
            aspect_ratio = max(w, h) / max(min(w, h), 1)
            if aspect_ratio > 1.5:  # 宽高比不应超过1.5
                print(f"    宽高比验证失败: {aspect_ratio:.2f} > 1.5")
                return False
            
            # 计算轮廓的等效圆半径
            equivalent_radius = np.sqrt(area / np.pi)
            
            # 计算轮廓到等效圆心的平均距离
            M = cv2.moments(contour)
            if M["m00"] == 0:
                print(f"    质心计算失败: m00 = 0")
                return False
            
            cx = M["m10"] / M["m00"]
            cy = M["m01"] / M["m00"]
            
            # 计算轮廓点到圆心的距离
            distances = []
            for point in contour:
                point = point[0]  # OpenCV轮廓点的格式
                dist = np.sqrt((point[0] - cx) ** 2 + (point[1] - cy) ** 2)
                distances.append(dist)
            
            if len(distances) == 0:
                print(f"    距离计算失败: 无有效点")
                return False
            
            # 计算距离的标准差（圆形光斑的距离应该相对一致）
            mean_distance = np.mean(distances)
            std_distance = np.std(distances)
            
            # 如果标准差相对于平均距离太大，说明不是圆形
            if mean_distance > 0 and std_distance / mean_distance > 0.3:
                print(f"    距离一致性验证失败: 标准差比 {std_distance/mean_distance:.3f} > 0.3")
                return False
            
            # 检查面积与周长的关系（圆形光斑应该符合 A = πr², P = 2πr）
            perimeter = cv2.arcLength(contour, True)
            if perimeter > 0:
                theoretical_radius = perimeter / (2 * np.pi)
                area_radius = np.sqrt(area / np.pi)
                radius_ratio = min(theoretical_radius, area_radius) / max(theoretical_radius, area_radius)
                if radius_ratio < 0.7:  # 理论半径与实际半径的比值应该接近1
                    print(f"    半径比例验证失败: {radius_ratio:.3f} < 0.7")
                    return False
            
            print(f"    ✓ 圆形光斑验证通过 - 宽高比: {aspect_ratio:.2f}, 距离一致性: {std_distance/mean_distance:.3f}, 半径比例: {radius_ratio:.3f}")
            return True
            
        except Exception as e:
            print(f"圆形光斑验证失败: {e}")
            return False
    
    def draw_blobs(self, image, contours, is_left_camera=True):
        """在图像上绘制光斑检测结果"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                # 计算质心
                M = cv2.moments(contour)
                if M["m00"] != 0:
                    cx = M["m10"] / M["m00"]
                    cy = M["m01"] / M["m00"]
                else:
                    cx, cy = 0, 0
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 20, int(cy) - 10), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 15, int(cy) + 30), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                
                # 如果有相机参数，计算并显示三维坐标
                if self.camera_params is not None:
                    # 这里需要左右相机配合计算，暂时只显示2D坐标
                    # 三维坐标计算需要在主程序中完成
                    pass
            
            return result_image
            
        except Exception as e:
            return image


class ThreeDCoordinateThread(QThread):
    """三维坐标计算线程 - 使用OpenCV最优算法计算单个发光标志的三维坐标"""
    
    # 信号定义
    coordinate_ready = pyqtSignal(object)  # 发送单个发光标志的三维坐标结果
    image_update_ready = pyqtSignal(object, object, object)  # 发送左图像、右图像、三维坐标用于更新显示
    
    def __init__(self, camera_params=None):
        super().__init__()
        self.camera_params = camera_params
        self.is_running = False
        self.left_centroids = []
        self.right_centroids = []
        self.left_contours = []
        self.right_contours = []
        self.left_image = None
        self.right_image = None
        self.last_point_3d = None  # 保存上一次的三维坐标，用于防闪烁
        self.last_update_time = 0  # 记录上次更新时间
        self.update_timeout = 2.0  # 2秒超时，确保遮挡移除后能更新
        self.mutex = threading.Lock()
        
        # 如果没有提供相机参数，尝试从JSON文件加载
        if self.camera_params is None:
            self.load_calibration_from_json()
    
    def load_calibration_from_json(self):
        """从JSON文件加载标定参数"""
        try:
            # 获取当前脚本所在目录
            script_dir = os.path.dirname(os.path.abspath(__file__))
            calibration_file = os.path.join(script_dir, "camera_calibration.json")
            
            if os.path.exists(calibration_file):
                with open(calibration_file, 'r', encoding='utf-8') as f:
                    self.camera_params = json.load(f)
                print("✓ 从JSON文件加载标定参数成功")
                print(f"  标定文件: {calibration_file}")
                
                # 显示标定参数信息
                if self.camera_params:
                    calib_info = self.camera_params.get("calibration_info", {})
                    print(f"  棋盘格尺寸: {calib_info.get('chessboard_size', 'Unknown')}")
                    print(f"  方格大小: {calib_info.get('square_size_mm', 'Unknown')} mm")
                    
                    left_cam = self.camera_params.get("left_camera", {})
                    right_cam = self.camera_params.get("right_camera", {})
                    stereo = self.camera_params.get("stereo_camera", {})
                    
                    if left_cam and right_cam and stereo:
                        print(f"  左相机焦距: fx={left_cam.get('camera_matrix', [[0]])[0][0]:.2f}, fy={left_cam.get('camera_matrix', [[0,0,0],[0,0,0],[0,0,1]])[1][1]:.2f}")
                        print(f"  右相机焦距: fx={right_cam.get('camera_matrix', [[0]])[0][0]:.2f}, fy={right_cam.get('camera_matrix', [[0,0,0],[0,0,0],[0,0,1]])[1][1]:.2f}")
                        baseline = abs(stereo.get('translation_vector', [[0]])[0][0])
                        print(f"  基线距离: {baseline:.2f} mm")
            else:
                print(f"✗ 标定文件不存在: {calibration_file}")
                print("  请先运行标定程序生成camera_calibration.json文件")
        except Exception as e:
            print(f"✗ 加载标定参数失败: {e}")
            self.camera_params = None
    
    def update_centroids(self, left_centroids, right_centroids):
        """更新质心坐标"""
        with self.mutex:
            self.left_centroids = left_centroids.copy()
            self.right_centroids = right_centroids.copy()
    
    def update_images(self, left_image, right_image):
        """更新图像数据"""
        with self.mutex:
            self.left_image = left_image.copy() if left_image is not None else None
            self.right_image = right_image.copy() if right_image is not None else None
    
    def update_contours(self, left_contours, right_contours):
        """更新轮廓数据"""
        with self.mutex:
            self.left_contours = left_contours.copy() if left_contours is not None else []
            self.right_contours = right_contours.copy() if right_contours is not None else []
    
    def run(self):
        """线程运行函数"""
        self.is_running = True
        
        while self.is_running:
            try:
                # 获取当前质心坐标和轮廓数据
                with self.mutex:
                    left_centroids = self.left_centroids.copy()
                    right_centroids = self.right_centroids.copy()
                    left_contours = self.left_contours.copy()
                    right_contours = self.right_contours.copy()
                
                # 检查是否有光斑检测结果和相机参数
                if len(left_centroids) > 0 and len(right_centroids) > 0 and self.camera_params is not None:
                    # 选择最佳匹配的光斑对
                    best_left_point, best_right_point = self.find_best_matching_blobs(left_centroids, right_centroids)
                    
                    if best_left_point is not None and best_right_point is not None:
                        # 使用OpenCV前方交会算法计算三维坐标
                        point_3d_opencv = self.calculate_3d_coordinates_opencv(best_left_point, best_right_point, self.camera_params)
                        
                        # 准备结果数据
                        result_data = {
                            'opencv': point_3d_opencv
                        }
                        
                        if point_3d_opencv is not None:
                            # 改进的防闪烁机制：检测光斑重新出现
                            current_time = time.time()
                            should_update = True
                            
                            if self.last_point_3d is not None:
                                # 计算坐标变化
                                diff = np.linalg.norm(np.array(point_3d_opencv) - np.array(self.last_point_3d))
                                time_since_update = current_time - self.last_update_time
                                
                                # 如果坐标变化很小且时间未超时，则不更新
                                if diff < 2.0 and time_since_update < self.update_timeout:
                                    should_update = False
                            
                            if should_update:
                                # 发送结果到界面
                                self.coordinate_ready.emit(result_data)
                                # 保存当前坐标和时间
                                self.last_point_3d = point_3d_opencv.copy()
                                self.last_update_time = current_time
                                print(f"✓ 三维坐标已更新: ({point_3d_opencv[0]:.2f}, {point_3d_opencv[1]:.2f}, {point_3d_opencv[2]:.2f})")
                        else:
                            # 计算失败，发送空结果
                            self.coordinate_ready.emit(result_data)
                            # 清除上一次的坐标，确保下次能正确更新
                            self.last_point_3d = None
                            self.last_update_time = 0
                    else:
                        # 没有找到匹配的光斑对，发送空结果
                        result_data = {
                            'opencv': None
                        }
                        self.coordinate_ready.emit(result_data)
                else:
                    # 没有光斑或相机参数，发送空结果
                    result_data = {
                        'opencv': None
                    }
                    self.coordinate_ready.emit(result_data)
                
                # 休眠一段时间，避免过度计算
                time.sleep(0.1)  # 100ms更新一次，提高响应速度
                
            except Exception as e:
                print(f"三维坐标计算线程错误: {e}")
                time.sleep(0.1)
    
    def find_best_matching_blobs(self, left_centroids, right_centroids):
        """找到最佳匹配的光斑对（增强版，减少闪烁影响）"""
        try:
            best_left = None
            best_right = None
            min_score = float('inf')
            
            # 如果没有检测到光斑，返回None
            if len(left_centroids) == 0 or len(right_centroids) == 0:
                return None, None
            
            # 动态调整匹配参数，基于光斑数量
            y_tolerance = max(20, min(50, 30 + len(left_centroids) * 2))  # 动态Y容差
            x_min_diff = max(5, min(20, 10 + len(left_centroids)))        # 动态最小X差异
            x_max_diff = max(200, min(400, 300 + len(left_centroids) * 10))  # 动态最大X差异
            
            for left_point in left_centroids:
                for right_point in right_centroids:
                    # 计算匹配分数（Y坐标差异应该很小）
                    y_diff = abs(left_point[1] - right_point[1])
                    
                    # 检查X坐标差异（右图像中的点应该在左图像中点的左侧）
                    x_diff = left_point[0] - right_point[0]
                    
                    # 使用动态容差参数
                    if y_diff < y_tolerance and x_diff > x_min_diff and x_diff < x_max_diff:
                        # 计算匹配分数，优先考虑Y坐标匹配，但增加位置稳定性权重
                        ideal_disparity = 120  # 假设理想视差约为120像素
                        score = y_diff * 2 + abs(x_diff - ideal_disparity) * 0.1
                        
                        # 如果这是当前最佳匹配，检查稳定性
                        if score < min_score:
                            # 检查是否与之前的最佳匹配接近（减少闪烁）
                            if best_left is not None and best_right is not None:
                                left_stability = self.calculate_stability_score(left_point, best_left)
                                right_stability = self.calculate_stability_score(right_point, best_right)
                                
                                # 如果新匹配更稳定，选择新匹配
                                if left_stability > 0.7 and right_stability > 0.7:
                                    min_score = score
                                    best_left = left_point
                                    best_right = right_point
                            else:
                                # 第一次匹配
                                min_score = score
                                best_left = left_point
                                best_right = right_point
            
            if best_left is not None and best_right is not None:
                print(f"匹配成功 - 左点: ({best_left[0]:.1f}, {best_left[1]:.1f})")
                print(f"匹配成功 - 右点: ({best_right[0]:.1f}, {best_right[1]:.1f})")
                print(f"匹配成功 - Y差异: {abs(best_left[1] - best_right[1]):.1f} 像素")
                print(f"匹配成功 - X视差: {best_left[0] - best_right[0]:.1f} 像素")
                print(f"匹配成功 - 使用容差: Y={y_tolerance}, X范围=[{x_min_diff}, {x_max_diff}]")
            
            return best_left, best_right
            
        except Exception as e:
            print(f"光斑匹配失败: {e}")
            return None, None
    
    def calculate_stability_score(self, new_point, old_point):
        """计算光斑稳定性分数"""
        try:
            # 计算位置距离
            distance = np.sqrt((new_point[0] - old_point[0])**2 + (new_point[1] - old_point[1])**2)
            
            # 距离越小，稳定性越高
            if distance < 5:
                return 1.0
            elif distance < 15:
                return 0.8
            elif distance < 25:
                return 0.6
            elif distance < 35:
                return 0.4
            else:
                return 0.2
                
        except Exception as e:
            print(f"计算稳定性分数失败: {e}")
            return 0.0
            
        except Exception as e:
            print(f"光斑匹配失败: {e}")
            return None, None
    

    
    def calculate_3d_coordinates_opencv(self, left_point, right_point, camera_params):
        """使用OpenCV前方交会算法计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机内参矩阵
            left_matrix = np.array(left_cam["camera_matrix"], dtype=np.float64)
            right_matrix = np.array(right_cam["camera_matrix"], dtype=np.float64)
            
            # 构建旋转矩阵和平移向量
            R = np.array(stereo["rotation_matrix"], dtype=np.float64)
            T = np.array(stereo["translation_vector"], dtype=np.float64).flatten()
            
            # 构建投影矩阵
            # 左相机投影矩阵: P1 = K1 * [I | 0]
            P1 = left_matrix @ np.hstack([np.eye(3), np.zeros((3, 1))])
            
            # 右相机投影矩阵: P2 = K2 * [R | t]
            P2 = right_matrix @ np.hstack([R, T.reshape(3, 1)])
            
            # 准备图像点坐标
            left_points = np.array([[left_point[0], left_point[1]]], dtype=np.float64)
            right_points = np.array([[right_point[0], right_point[1]]], dtype=np.float64)
            
            # 使用OpenCV的triangulatePoints进行前方交会
            points_4d = cv2.triangulatePoints(P1, P2, left_points.T, right_points.T)
            
            # 转换为3D坐标（齐次坐标转欧几里得坐标）
            point_3d = points_4d[:3, 0] / points_4d[3, 0]
            
            # 打印调试信息
            print(f"OpenCV算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"OpenCV算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"OpenCV算法 - 三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            print(f"OpenCV算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
            
            return point_3d
            
        except Exception as e:
            print(f"OpenCV前方交会算法计算失败: {e}")
            return None
    
    def calculate_3d_coordinates_simple(self, left_point, right_point, camera_params):
        """使用简单的双目立体视觉公式计算三维坐标（以左眼相机为原点）"""
        try:
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 获取基线距离（左右相机之间的距离）
            baseline = stereo["translation"]["x"]  # 通常是x方向的平移
            
            # 获取相机的内参
            fx = cam["fx"]  # 像素焦距
            fy = cam["fy"]  # 像素焦距
            cx = cam["cx"]  # 主点x坐标
            cy = cam["cy"]  # 主点y坐标
            
            # 计算视差（disparity）
            # 在标准双目视觉中，左图像中的点在右图像中应该向左偏移
            disparity = left_point[0] - right_point[0]
            
            # 使用标准双目公式计算深度
            if abs(disparity) > 0.1:  # 避免除零
                # 计算深度 Z = (fx * baseline) / disparity
                Z = (fx * baseline) / disparity
                
                # 计算X和Y坐标（在左相机坐标系中）
                X = (left_point[0] - cx) * Z / fx
                Y = (left_point[1] - cy) * Z / fy
                
                point_3d = np.array([X, Y, Z])
                
                # 打印调试信息
                print(f"简单算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
                print(f"简单算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
                print(f"简单算法 - 视差: {disparity:.2f} 像素")
                print(f"简单算法 - 基线距离: {baseline:.2f} mm")
                print(f"简单算法 - 三维坐标: ({X:.2f}, {Y:.2f}, {Z:.2f})")
                print(f"简单算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
                
                return point_3d
            else:
                print("视差太小，无法计算三维坐标")
                return None
                
        except Exception as e:
            print(f"简单三维坐标计算失败: {e}")
            return None
    
    def analyze_algorithm_differences(self, left_point, right_point, point_3d_simple, point_3d_opencv):
        """分析两种算法的差异原因"""
        try:
            print(f"\n=== 算法差异分析 ===")
            
            # 计算各坐标分量的差异
            x_diff = abs(point_3d_simple[0] - point_3d_opencv[0])
            y_diff = abs(point_3d_simple[1] - point_3d_opencv[1])
            z_diff = abs(point_3d_simple[2] - point_3d_opencv[2])
            
            print(f"X坐标差异: {x_diff:.1f}mm")
            print(f"Y坐标差异: {y_diff:.1f}mm")
            print(f"Z坐标差异: {z_diff:.1f}mm")
            
            # 分析可能的原因
            print(f"\n=== 差异原因分析 ===")
            
            # 1. 算法原理差异
            print("1. 算法原理差异:")
            print("   - 简单算法: 使用标准双目公式 Z = (fx * baseline) / disparity")
            print("   - OpenCV算法: 使用前方交会三角测量，考虑完整的投影矩阵")
            
            # 2. 坐标系处理差异
            print("2. 坐标系处理差异:")
            print("   - 简单算法: 直接使用左相机坐标系")
            print("   - OpenCV算法: 通过投影矩阵变换，可能涉及坐标系转换")
            
            # 3. 参数使用差异
            print("3. 参数使用差异:")
            print("   - 简单算法: 使用统一相机内参和基线距离")
            print("   - OpenCV算法: 使用统一相机内参、旋转矩阵、平移向量")
            
            # 4. 精度差异
            print("4. 精度差异:")
            print("   - 简单算法: 假设理想的双目几何关系")
            print("   - OpenCV算法: 考虑实际的相机几何关系，包括旋转和平移")
            
            # 5. 数值计算差异
            print("5. 数值计算差异:")
            print("   - 简单算法: 直接代数计算")
            print("   - OpenCV算法: 使用SVD分解等数值方法")
            
            # 判断哪种算法更可靠
            print(f"\n=== 算法可靠性评估 ===")
            
            # 检查视差
            disparity = left_point[0] - right_point[0]
            print(f"视差: {disparity:.2f} 像素")
            
            if abs(disparity) < 5:
                print("警告: 视差太小，可能导致计算不稳定")
            elif abs(disparity) > 200:
                print("警告: 视差太大，可能超出有效测量范围")
            else:
                print("视差在合理范围内")
            
            # 检查深度值
            z_simple = point_3d_simple[2]
            z_opencv = point_3d_opencv[2]
            
            print(f"简单算法深度: {z_simple:.1f}mm")
            print(f"OpenCV算法深度: {z_opencv:.1f}mm")
            
            if z_simple < 0 or z_opencv < 0:
                print("警告: 深度为负值，可能表示计算错误")
            elif z_simple > 10000 or z_opencv > 10000:
                print("警告: 深度值过大，可能超出合理范围")
            else:
                print("深度值在合理范围内")
            
            # 推荐算法
            print(f"\n=== 算法推荐 ===")
            if abs(z_diff) < 50:  # 如果深度差异小于50mm
                print("两种算法结果接近，都可信")
                if abs(disparity) < 20:
                    print("推荐使用OpenCV算法（视差较小，需要更高精度）")
                else:
                    print("推荐使用简单算法（视差较大，计算稳定）")
            else:
                print("两种算法结果差异较大，需要进一步验证")
                if z_simple > 0 and z_opencv < 0:
                    print("推荐使用简单算法（OpenCV算法深度为负）")
                elif z_opencv > 0 and z_simple < 0:
                    print("推荐使用OpenCV算法（简单算法深度为负）")
                else:
                    print("建议检查相机标定参数和图像质量")
            
        except Exception as e:
            print(f"算法差异分析失败: {e}")
    
    def draw_image_with_3d_coordinates(self, image, contours, centroids, point_3d, camera_name):
        """在图像上绘制光斑和三维坐标"""
        try:
            result_image = image.copy()
            
            # 绘制所有检测到的光斑
            for i, (contour, centroid) in enumerate(zip(contours, centroids)):
                cx, cy = centroid
                
                # 计算面积
                area = cv2.contourArea(contour)
                
                # 绘制光斑轮廓（红色圆圈）
                cv2.circle(result_image, (int(cx), int(cy)), 10, (0, 0, 255), 2)
                
                # 绘制十字线（绿色）
                cv2.line(result_image, (int(cx-15), int(cy)), (int(cx+15), int(cy)), (0, 255, 0), 2)
                cv2.line(result_image, (int(cx), int(cy-15)), (int(cx), int(cy+15)), (0, 255, 0), 2)
                
                # 显示质心坐标（绿色，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                
                # 显示面积（黄色，增大字体，位置调整避免重叠）
                area_text = f"A:{area:.0f}"
                cv2.putText(result_image, area_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 3)
                
                # 在光斑旁边显示三维坐标（白色）
                if point_3d is not None:
                    coord_3d_text = f"X:{point_3d[0]:.1f} Y:{point_3d[1]:.1f} Z:{point_3d[2]:.1f}"
                    
                    # 在光斑右侧显示三维坐标
                    text_x = int(cx) + 20
                    text_y = int(cy) + 20
                    
                    # 绘制三维坐标文本
                    cv2.putText(result_image, coord_3d_text, (text_x, text_y), 
                               cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_image_with_3d_coordinates_simple(self, image, centroids, point_3d, camera_name):
        """在图像上绘制光斑和三维坐标（简化版本，解决闪烁问题）"""
        try:
            result_image = image.copy()
            
            # 绘制所有检测到的光斑
            for i, centroid in enumerate(centroids):
                cx, cy = centroid
                
                # 绘制光斑轮廓（红色圆圈）
                cv2.circle(result_image, (int(cx), int(cy)), 10, (0, 0, 255), 2)
                
                # 绘制十字线（绿色）
                cv2.line(result_image, (int(cx-15), int(cy)), (int(cx+15), int(cy)), (0, 255, 0), 2)
                cv2.line(result_image, (int(cx), int(cy-15)), (int(cx), int(cy+15)), (0, 255, 0), 2)
                
                # 显示质心坐标（绿色）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 15, int(cy) - 15), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
                
                # 在光斑旁边显示三维坐标（白色）
                if point_3d is not None:
                    coord_3d_text = f"X:{point_3d[0]:.1f} Y:{point_3d[1]:.1f} Z:{point_3d[2]:.1f}"
                    
                    # 在光斑右侧显示三维坐标
                    text_x = int(cx) + 20
                    text_y = int(cy) + 20
                    
                    # 绘制三维坐标文本
                    cv2.putText(result_image, coord_3d_text, (text_x, text_y), 
                               cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def stop(self):
        """停止线程"""
        self.is_running = False


class ZEDCameraThread(QThread):
    """ZED相机图像捕获线程 - 专门负责图像捕获和光斑检测"""
    
    # 信号定义
    image_processed_ready = pyqtSignal(object, object, list, list, list, list)  # 发送处理后的图像和光斑检测结果
    exposure_changed = pyqtSignal(int)
    error_occurred = pyqtSignal(str)
    
    def __init__(self, zed_camera, main_window=None):
        super().__init__()
        self.zed = zed_camera
        self.main_window = main_window  # 保存主窗口引用
        self.is_running = False
        self.current_exposure = 0
        self.min_exposure = 0
        self.max_exposure = 0
        
        # 聚焦控制
        self.auto_focus_enabled = True  # 默认启用自动聚焦
        
        # 初始化光斑检测器
        self.blob_detector = BlobDetector()
        self.blob_enabled = True  # 默认启用光斑检测
        
                            # 光斑检测器初始化
        print(f"光斑检测器初始化 - 最小面积: {self.blob_detector.min_area}, 最大面积: {self.blob_detector.max_area}")
        print(f"光斑检测器参数 - 阈值: {self.blob_detector.threshold_value}")
        print("✓ 已启用简化光斑检测模式（仅面积约束）")
        
        # 获取曝光范围
        try:
            exposure_info = self.zed.get_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE)
            if len(exposure_info) >= 3:
                self.current_exposure = exposure_info[0]
                self.min_exposure = exposure_info[1]
                self.max_exposure = exposure_info[2]
            else:
                # 如果获取失败，使用默认值
                self.min_exposure = 0
                self.max_exposure = 100
                self.current_exposure = 50
        except Exception as e:
            # 使用默认值
            self.min_exposure = 0
            self.max_exposure = 100
            self.current_exposure = 50
    
    def set_exposure(self, exposure_value):
        """设置曝光值"""
        try:
            # 确保曝光值在有效范围内
            exposure_value = max(self.min_exposure, min(self.max_exposure, exposure_value))
            
            # 设置曝光
            result = self.zed.set_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE, exposure_value)
            
            # 检查设置是否成功
            if result == sl.ERROR_CODE.SUCCESS:
                self.current_exposure = exposure_value
                # 发送信号通知曝光变化
                self.exposure_changed.emit(exposure_value)
                print(f"曝光已设置为: {exposure_value}")
                return True
            else:
                print(f"设置曝光失败，错误代码: {result}")
                return False
        except Exception as e:
            print(f"设置曝光异常: {e}")
            return False
    
    def set_auto_focus(self, enabled):
        """设置自动聚焦"""
        try:
            if enabled:
                # 启用自动聚焦
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC_ROI, sl.Rect(0, 0, 0, 0))  # 全区域自动聚焦
            else:
                # 禁用自动聚焦
                self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
            
            self.auto_focus_enabled = enabled
            return True
        except Exception as e:
            return False
    
    def get_current_exposure(self):
        """获取当前曝光值"""
        try:
            # 使用更安全的方法获取曝光值
            exposure_info = self.zed.get_camera_settings(sl.VIDEO_SETTINGS.EXPOSURE)
            
            # 检查返回值类型
            if isinstance(exposure_info, tuple) and len(exposure_info) >= 1:
                exposure_value = exposure_info[0]
                if isinstance(exposure_value, (int, float)):
                    return int(exposure_value)
                elif hasattr(exposure_value, 'value'):  # 如果是枚举类型
                    return int(exposure_value.value)
                else:
                    return self.current_exposure
            elif isinstance(exposure_info, (int, float)):
                return int(exposure_info)
            else:
                return self.current_exposure
                
        except Exception as e:
            print(f"获取曝光值失败: {e}")
            return self.current_exposure
    
    def trigger_manual_focus(self):
        """触发手动聚焦"""
        try:
            # 保存当前曝光设置
            current_exposure = self.get_current_exposure()
            print(f"手动聚焦前保存曝光值: {current_exposure}")
            
            # 使用更安全的聚焦方法，只调整聚焦，不改变曝光
            try:
                # 简化的聚焦方法：只重新触发自动聚焦，不改变其他设置
                if self.auto_focus_enabled:
                    # 如果当前是自动聚焦，先禁用再启用来重新聚焦
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
                    time.sleep(0.1)
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                    time.sleep(0.5)  # 等待聚焦完成
                else:
                    # 如果当前是手动聚焦，临时启用自动聚焦
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, True)
                    time.sleep(0.5)  # 等待聚焦完成
                    self.zed.set_camera_settings(sl.VIDEO_SETTINGS.AEC_AGC, False)
                
                # 强制恢复曝光设置
                print(f"聚焦完成后恢复曝光值: {current_exposure}")
                if isinstance(current_exposure, (int, float)):
                    success = self.set_exposure(int(current_exposure))
                    if success:
                        print(f"✓ 曝光值恢复成功: {current_exposure}")
                    else:
                        print(f"⚠ 曝光值恢复失败: {current_exposure}")
                
                return True
                
            except Exception as focus_error:
                print(f"聚焦过程出错: {focus_error}")
                # 确保曝光设置不变
                if isinstance(current_exposure, (int, float)):
                    self.set_exposure(int(current_exposure))
                return True
                
        except Exception as e:
            print(f"手动聚焦失败: {e}")
            return False
    
    def run(self):
        """线程运行函数"""
        self.is_running = True

        # 创建图像矩阵
        image_left = sl.Mat()
        image_right = sl.Mat()
        runtime_parameters = sl.RuntimeParameters()

        while self.is_running:
            try:
                if self.zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
                    # 获取左右眼图像
                    self.zed.retrieve_image(image_left, sl.VIEW.LEFT)
                    self.zed.retrieve_image(image_right, sl.VIEW.RIGHT)
                    
                    # 转换为OpenCV格式
                    left_cv_image = np.array(image_left.get_data())
                    right_cv_image = np.array(image_right.get_data())
                    
                    # 确保图像是3通道RGB格式
                    if left_cv_image.shape[-1] == 4:
                        left_cv_image = cv2.cvtColor(left_cv_image, cv2.COLOR_RGBA2RGB)
                    if right_cv_image.shape[-1] == 4:
                        right_cv_image = cv2.cvtColor(right_cv_image, cv2.COLOR_RGBA2RGB)
                    
                    # 转换为BGR格式（OpenCV默认格式）
                    left_bgr_original = cv2.cvtColor(left_cv_image, cv2.COLOR_RGB2BGR)
                    right_bgr_original = cv2.cvtColor(right_cv_image, cv2.COLOR_RGB2BGR)
                    
                    # 获取原始图像尺寸
                    left_height, left_width = left_bgr_original.shape[:2]
                    right_height, right_width = right_bgr_original.shape[:2]
                    
                    # 光斑检测（如果启用）- 使用原始分辨率进行检测
                    left_contours = []
                    right_contours = []
                    left_centroids = []
                    right_centroids = []
                    
                    if hasattr(self, 'blob_enabled') and self.blob_enabled:
                        left_contours, _ = self.blob_detector.detect_blobs(left_bgr_original)
                        right_contours, _ = self.blob_detector.detect_blobs(right_bgr_original)
                        
                        # 计算质心
                        for contour in left_contours:
                            M = cv2.moments(contour)
                            if M["m00"] != 0:
                                cx = M["m10"] / M["m00"]
                                cy = M["m01"] / M["m00"]
                                left_centroids.append((cx, cy))
                        
                        for contour in right_contours:
                            M = cv2.moments(contour)
                            if M["m00"] != 0:
                                cx = M["m10"] / M["m00"]
                                cy = M["m01"] / M["m00"]
                                right_centroids.append((cx, cy))
                        
                        # 光斑稳定性增强：如果检测到的光斑数量不稳定，使用历史数据
                        if hasattr(self, '_left_centroids_history'):
                            if len(left_centroids) == 0 and len(self._left_centroids_history) > 0:
                                # 如果当前帧没有检测到光斑，使用最近的历史数据
                                left_centroids = self._left_centroids_history[-1] if self._left_centroids_history else []
                            
                            if len(right_centroids) == 0 and len(self._right_centroids_history) > 0:
                                # 如果当前帧没有检测到光斑，使用最近的历史数据
                                right_centroids = self._right_centroids_history[-1] if self._right_centroids_history else []
                        
                        # 更新历史数据
                        if not hasattr(self, '_left_centroids_history'):
                            self._left_centroids_history = []
                            self._right_centroids_history = []
                        
                        if len(left_centroids) > 0:
                            self._left_centroids_history.append(left_centroids)
                        if len(right_centroids) > 0:
                            self._right_centroids_history.append(right_centroids)
                        
                        # 保持历史数据长度
                        if len(self._left_centroids_history) > 3:
                            self._left_centroids_history.pop(0)
                        if len(self._right_centroids_history) > 3:
                            self._right_centroids_history.pop(0)
                    
                    # 绘制检测结果 - 使用原始分辨率
                    left_result = left_bgr_original.copy()
                    right_result = right_bgr_original.copy()
                    
                    if hasattr(self, 'blob_enabled') and self.blob_enabled:
                        # 在图像上绘制光斑检测结果
                        if len(left_contours) > 0 and len(left_centroids) > 0:
                            left_result = self.draw_blobs_simple(left_bgr_original, left_contours, left_centroids, "Left")
                            print(f"左眼绘制光斑: {len(left_contours)} 个轮廓, {len(left_centroids)} 个质心")
                        else:
                            print(f"左眼无光斑: {len(left_contours)} 个轮廓, {len(left_centroids)} 个质心")
                            
                        if len(right_contours) > 0 and len(right_centroids) > 0:
                            right_result = self.draw_blobs_simple(right_bgr_original, right_contours, right_centroids, "Right")
                            print(f"右眼绘制光斑: {len(right_contours)} 个轮廓, {len(right_centroids)} 个质心")
                        else:
                            print(f"右眼无光斑: {len(right_contours)} 个轮廓, {len(right_centroids)} 个质心")
                    else:
                        print("光斑检测已禁用")
                    
                    # 发送处理后的图像和光斑检测结果到图像显示线程
                    self.image_processed_ready.emit(
                        left_result, right_result,
                        left_centroids, right_centroids,
                        left_contours, right_contours
                    )
                    
                    # 调试信息（仅在开始时打印）
                    if not hasattr(self, '_debug_printed'):
                        self._debug_printed = True
                
                time.sleep(0.033)  # 约30FPS
                
            except Exception as e:
                time.sleep(0.1)
    
    def draw_blobs_simple(self, image, contours, centroids, camera_name):
        """绘制光斑（简化版本，不包含三维坐标）"""
        try:
            result_image = image.copy()
            
            # 检查是否有轮廓和质心数据
            if len(contours) == 0 or len(centroids) == 0:
                print(f"{camera_name} 相机: 无光斑数据，返回原始图像")
                return result_image
            
            print(f"{camera_name} 相机: 开始绘制 {len(contours)} 个光斑")
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    print(f"{camera_name} 相机: 轮廓 {i} 超出质心范围")
                    continue
                    
                cx, cy = centroids[i]
                print(f"{camera_name} 相机: 绘制光斑 {i+1}, 质心: ({cx:.1f}, {cy:.1f})")
                
                # 计算面积
                area = cv2.contourArea(contour)
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 显示质心坐标（绿色，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
                
                # 显示面积（黄色，增大字体，位置调整避免重叠）
                area_text = f"A:{area:.0f}"
                cv2.putText(result_image, area_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 3)
                
                print(f"{camera_name} 相机: 光斑 {i+1} 绘制完成, 面积: {area:.0f}")
            
            print(f"{camera_name} 相机: 所有光斑绘制完成")
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_blobs_with_3d_simple(self, image, contours, centroids, three_d_coordinates, camera_name):
        """绘制光斑并显示三维坐标（简化版本）"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    continue
                    
                cx, cy = centroids[i]
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                
                # 查找并显示三维坐标
                for idx, point_3d in three_d_coordinates:
                    if idx == i:
                        # 显示三维坐标（白色，稍大字体）
                        x_coord = point_3d[0]
                        y_coord = point_3d[1]
                        z_coord = point_3d[2]
                        
                        # 格式化坐标显示
                        coord_3d_text = f"3D: X{x_coord:.1f} Y{y_coord:.1f} Z{z_coord:.1f}"
                        
                        # 在光斑下方显示三维坐标
                        text_x = max(5, int(cx) - 50)  # 确保文本不超出图像边界
                        text_y = int(cy) + 35
                        
                        # 绘制白色文本，稍大字体
                        cv2.putText(result_image, coord_3d_text, 
                                   (text_x, text_y), 
                                   cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
                break
            
            return result_image
            
        except Exception as e:
            print(f"Draw {camera_name} camera image failed: {e}")
            return image
    
    def draw_blobs_with_3d(self, image, contours, centroids, point_3d, is_left_camera):
        """绘制光斑并显示三维坐标"""
        try:
            result_image = image.copy()
            
            for i, contour in enumerate(contours):
                if i >= len(centroids):
                    continue
                    
                cx, cy = centroids[i]
                
                # 计算轮廓的边界框
                x, y, w, h = cv2.boundingRect(contour)
                size = max(w, h)
                
                # 绘制轮廓
                cv2.drawContours(result_image, [contour], -1, (255, 0, 0), 2)
                
                # 绘制十字线（更细）
                color = (0, 255, 0)  # 绿色
                thickness = 1  # 更细的线条
                length = max(8, size // 3)
                
                # 水平线
                cv2.line(result_image, (int(cx - length), int(cy)), (int(cx + length), int(cy)), color, thickness)
                # 垂直线
                cv2.line(result_image, (int(cx), int(cy - length)), (int(cx), int(cy + length)), color, thickness)
                
                # 绘制质心坐标文本（保留一位小数，增大字体，位置调整避免重叠）
                coord_text = f"({cx:.1f}, {cy:.1f})"
                cv2.putText(result_image, coord_text, (int(cx) + 25, int(cy) - 20), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.2, color, 3)
                
                # 绘制光斑编号和面积（增大字体，位置调整避免重叠）
                area = cv2.contourArea(contour)
                info_text = f"#{i+1} A:{area:.0f}"
                cv2.putText(result_image, info_text, (int(cx) - 20, int(cy) + 35), 
                           cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0), 3)
                

            
            return result_image
            
        except Exception as e:
            return image
    
    def calculate_3d_coordinates(self, left_point, right_point, camera_params):
        """根据左右图像中的点计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机矩阵
            left_matrix = np.array([
                [left_cam["fx"], 0, left_cam["cx"]],
                [0, left_cam["fy"], left_cam["cy"]],
                [0, 0, 1]
            ])
            
            right_matrix = np.array([
                [right_cam["fx"], 0, right_cam["cx"]],
                [0, right_cam["fy"], right_cam["cy"]],
                [0, 0, 1]
            ])
            
            # 构建旋转矩阵
            R = np.array([
                [stereo["rotation"]["r00"], stereo["rotation"]["r01"], stereo["rotation"]["r02"]],
                [stereo["rotation"]["r10"], stereo["rotation"]["r11"], stereo["rotation"]["r12"]],
                [stereo["rotation"]["r20"], stereo["rotation"]["r21"], stereo["rotation"]["r22"]]
            ])
            
            # 构建平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]])
            
            # 打印相机参数信息
            print(f"左相机矩阵:\n{left_matrix}")
            print(f"右相机矩阵:\n{right_matrix}")
            print(f"旋转矩阵:\n{R}")
            print(f"平移向量: {T}")
            
            # 使用OpenCV的triangulatePoints函数
            # 准备点坐标
            left_points = np.array([[left_point[0], left_point[1]]], dtype=np.float32)
            right_points = np.array([[right_point[0], right_point[1]]], dtype=np.float32)
            
            # 构建投影矩阵
            left_proj = left_matrix @ np.hstack([np.eye(3), np.zeros((3, 1))])
            right_proj = right_matrix @ np.hstack([R, T.reshape(3, 1)])
            
            print(f"左投影矩阵:\n{left_proj}")
            print(f"右投影矩阵:\n{right_proj}")
            
            # 使用OpenCV的triangulatePoints
            points_4d = cv2.triangulatePoints(left_proj, right_proj, left_points.T, right_points.T)
            
            # 转换为3D坐标
            point_3d = points_4d[:3, 0] / points_4d[3, 0]
            
            # 打印调试信息
            print(f"左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            
            # 坐标系说明
            print("坐标系定义:")
            print("X轴: 指向右方向（从相机中心向右）")
            print("Y轴: 指向下方向（从相机中心向下）")
            print("Z轴: 指向前方（从相机中心向前，即光轴方向）")
            print("Z轴为负值表示物体在相机前方，这是正常的")
            
            return point_3d
            
        except Exception as e:
            print(f"计算三维坐标失败: {e}")
            return None
    
    def calculate_3d_coordinates_simple(self, left_point, right_point, camera_params):
        """使用简单的双目立体视觉公式计算三维坐标（以左眼相机为原点）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 获取基线距离（左右相机之间的距离）
            baseline = abs(stereo["translation_vector"][0][0])  # X方向平移的绝对值
            
            # 获取左相机内参
            fx = left_cam["camera_matrix"][0][0]  # 焦距
            fy = left_cam["camera_matrix"][1][1]  # 焦距
            cx = left_cam["camera_matrix"][0][2]  # 主点x坐标
            cy = left_cam["camera_matrix"][1][2]  # 主点y坐标
            
            # 计算视差
            disparity = left_point[0] - right_point[0]
            
            if abs(disparity) < 0.1:
                print("视差太小，无法计算")
                return None
            
            # 使用双目公式计算深度
            Z = (fx * baseline) / disparity
            
            # 计算X和Y坐标
            X = (left_point[0] - cx) * Z / fx
            Y = (left_point[1] - cy) * Z / fy
            
            point_3d = np.array([X, Y, Z])
            
            # 打印调试信息
            print(f"简单算法 - 左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"简单算法 - 右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"简单算法 - 视差: {disparity:.2f} 像素")
            print(f"简单算法 - 基线距离: {baseline:.2f} mm")
            print(f"简单算法 - 三维坐标: ({X:.2f}, {Y:.2f}, {Z:.2f})")
            print(f"简单算法 - 坐标系: 以左眼相机为原点，X右Y下Z前")
            
            return point_3d
            
        except Exception as e:
            print(f"简单三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_from_left_camera(self, image_point, camera_params):
        """从左相机图像点计算三维坐标（假设固定深度）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取左相机参数
            left_cam = camera_params["left_camera"]
            
            # 构建相机内参矩阵
            fx = left_cam["camera_matrix"][0][0]
            fy = left_cam["camera_matrix"][1][1]
            cx = left_cam["camera_matrix"][0][2]
            cy = left_cam["camera_matrix"][1][2]
            
            # 假设光斑在固定深度（比如1000mm）
            Z = 1000.0  # 固定深度，单位mm
            
            # 使用针孔相机模型计算X和Y
            X = (image_point[0] - cx) * Z / fx
            Y = (image_point[1] - cy) * Z / fy
            
            return np.array([X, Y, Z])
            
        except Exception as e:
            print(f"左相机三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_from_right_camera(self, image_point, camera_params):
        """从右相机图像点计算三维坐标（转换到左相机坐标系）"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建右相机内参矩阵
            fx = right_cam["camera_matrix"][0][0]
            fy = right_cam["camera_matrix"][1][1]
            cx = right_cam["camera_matrix"][0][2]
            cy = right_cam["camera_matrix"][1][2]
            
            # 假设光斑在固定深度（比如1000mm）
            Z = 1000.0  # 固定深度，单位mm
            
            # 在右相机坐标系下计算X和Y
            X_right = (image_point[0] - cx) * Z / fx
            Y_right = (image_point[1] - cy) * Z / fy
            
            # 构建旋转矩阵（从欧拉角）
            rx, ry, rz = stereo["rotation"]["rx"], stereo["rotation"]["ry"], stereo["rotation"]["rz"]
            
            Rx = np.array([[1, 0, 0],
                          [0, np.cos(rx), -np.sin(rx)],
                          [0, np.sin(rx), np.cos(rx)]])
            
            Ry = np.array([[np.cos(ry), 0, np.sin(ry)],
                          [0, 1, 0],
                          [-np.sin(ry), 0, np.cos(ry)]])
            
            Rz = np.array([[np.cos(rz), -np.sin(rz), 0],
                          [np.sin(rz), np.cos(rz), 0],
                          [0, 0, 1]])
            
            R = Rz @ Ry @ Rx
            
            # 平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]])
            
            # 将右相机坐标系下的点转换到左相机坐标系
            point_right = np.array([X_right, Y_right, Z])
            point_left = R @ point_right + T
            
            return point_left
            
        except Exception as e:
            print(f"右相机三维坐标计算失败: {e}")
            return None
    
    def calculate_3d_coordinates_advanced(self, left_point, right_point, camera_params):
        """使用OpenCV高级立体视觉算法计算三维坐标"""
        try:
            if camera_params is None:
                print("相机参数未提供")
                return None
            
            # 提取相机参数
            left_cam = camera_params["left_camera"]
            right_cam = camera_params["right_camera"]
            stereo = camera_params["stereo_camera"]
            
            # 构建相机矩阵
            left_matrix = np.array([
                [left_cam["fx"], 0, left_cam["cx"]],
                [0, left_cam["fy"], left_cam["cy"]],
                [0, 0, 1]
            ], dtype=np.float32)
            
            right_matrix = np.array([
                [right_cam["fx"], 0, right_cam["cx"]],
                [0, right_cam["fy"], right_cam["cy"]],
                [0, 0, 1]
            ], dtype=np.float32)
            
            # 构建旋转矩阵
            R = np.array([
                [stereo["rotation"]["r00"], stereo["rotation"]["r01"], stereo["rotation"]["r02"]],
                [stereo["rotation"]["r10"], stereo["rotation"]["r11"], stereo["rotation"]["r12"]],
                [stereo["rotation"]["r20"], stereo["rotation"]["r21"], stereo["rotation"]["r22"]]
            ], dtype=np.float32)
            
            # 构建平移向量
            T = np.array([stereo["translation"]["x"], 
                         stereo["translation"]["y"], 
                         stereo["translation"]["z"]], dtype=np.float32)
            
            # 计算空间直线交会
            point_3d = self.intersect_rays_least_squares(left_point, right_point, 
                                                       left_matrix, right_matrix, R, T)
            
            print(f"空间直线交会算法结果:")
            print(f"左图像点: ({left_point[0]:.2f}, {left_point[1]:.2f})")
            print(f"右图像点: ({right_point[0]:.2f}, {right_point[1]:.2f})")
            print(f"三维坐标: ({point_3d[0]:.2f}, {point_3d[1]:.2f}, {point_3d[2]:.2f})")
            print(f"坐标系: X右 Y下 Z前（正值）")
            
            return point_3d
            
        except Exception as e:
            print(f"高级三维坐标计算失败: {e}")
            return None
    

    

    

    
    def stop(self):
        """停止线程"""
        self.is_running = False


class FullSizeImageDialog(QDialog):
    """原尺寸图像显示对话框"""
    
    def __init__(self, image, title="原尺寸图像", parent=None, camera_thread=None):
        super().__init__(parent)
        self.setWindowTitle(title)
        self.setModal(False)  # 非模态对话框
        self.camera_thread = camera_thread
        self.is_left_camera = "左眼" in title
        self.main_window = None
        
        # 查找主窗口
        parent_widget = parent
        while parent_widget:
            if hasattr(parent_widget, 'camera_thread'):
                self.main_window = parent_widget
            break
            parent_widget = parent_widget.parent()
        
        # 设置窗口大小和位置
        screen = QApplication.primaryScreen().geometry()
        window_width = int(min(screen.width() * 0.8, image.shape[1]))
        window_height = int(min(screen.height() * 0.8, image.shape[0]))
        self.resize(window_width, window_height)
        
        # 创建布局
        layout = QVBoxLayout(self)
        
        # 创建滚动区域
        scroll_area = QScrollArea()
        scroll_area.setWidgetResizable(True)
        layout.addWidget(scroll_area)
        
        # 创建图像标签
        self.image_label = QLabel()
        
        # 设置标签大小策略
        self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        self.image_label.setAlignment(Qt.AlignCenter)
        self.image_label.setMinimumSize(100, 100)  # 设置最小尺寸
        
        # 转换图像格式并显示
        self.update_image_display(image)
        
        # 将标签添加到滚动区域
        scroll_area.setWidget(self.image_label)
        
        # 添加分辨率信息标签
        height, width, channel = image.shape
        self.resolution_label = QLabel(f"相机真实分辨率: {width} × {height} 像素")
        self.resolution_label.setAlignment(Qt.AlignCenter)
        self.resolution_label.setStyleSheet("font-size: 14px; font-weight: bold; color: #333; padding: 5px; background-color: #f0f0f0; border: 1px solid #ccc;")
        layout.addWidget(self.resolution_label)
        
        # 添加按钮布局
        button_layout = QHBoxLayout()
        
        # 实时刷新开关
        self.refresh_checkbox = QCheckBox("实时刷新")
        self.refresh_checkbox.setChecked(True)
        self.refresh_checkbox.toggled.connect(self.toggle_refresh)
        button_layout.addWidget(self.refresh_checkbox)
        
        # 最大化按钮
        maximize_btn = QPushButton("最大化")
        maximize_btn.setMaximumWidth(80)
        maximize_btn.clicked.connect(self.toggle_maximize)
        button_layout.addWidget(maximize_btn)
        
        # 关闭按钮
        close_btn = QPushButton("关闭")
        close_btn.setMaximumWidth(80)
        close_btn.clicked.connect(self.close)
        button_layout.addWidget(close_btn)
        
        layout.addLayout(button_layout)
        
        # 保存原始窗口大小
        self.original_size = None
        self.is_maximized = False
        
        # 设置定时器用于实时刷新
        self.refresh_timer = QTimer()
        self.refresh_timer.timeout.connect(self.update_image)
        self.refresh_timer.start(100)  # 每100ms刷新一次
    
    def toggle_maximize(self):
        """切换最大化状态"""
        if hasattr(self, 'is_maximized') and self.is_maximized:
            # 恢复原始大小
            if hasattr(self, 'original_size') and self.original_size:
                self.resize(self.original_size)
            self.is_maximized = False
        else:
            # 保存当前大小并最大化
            self.original_size = self.size()
            self.showMaximized()
            self.is_maximized = True
            
        # 延迟更新图像显示，确保窗口状态已经调整完成
        QTimer.singleShot(200, self.force_update_image)
    
    def toggle_refresh(self, enabled):
        """切换实时刷新"""
        if enabled:
            self.refresh_timer.start(100)
        else:
            self.refresh_timer.stop()
    
    def update_image_display(self, image):
        """更新图像显示"""
        try:
            # 确保图像是BGR格式
            if len(image.shape) == 3 and image.shape[2] == 3:
                # 转换为RGB格式用于QImage显示
                rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
                height, width, channel = rgb_image.shape
                bytes_per_line = 3 * width
                q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
                pixmap = QPixmap.fromImage(q_image)
                
                # 根据窗口状态调整图像大小
                if hasattr(self, 'is_maximized') and self.is_maximized:
                    # 最大化时，让图像适应窗口大小，保持宽高比
                    window_size = self.size()
                    scaled_pixmap = pixmap.scaled(
                        window_size.width() - 50,  # 留一些边距
                        window_size.height() - 100,  # 留一些边距给按钮和标签
                        Qt.KeepAspectRatio,
                        Qt.SmoothTransformation
                    )
                    self.image_label.setPixmap(scaled_pixmap)
                else:
                    # 非最大化时，显示原尺寸
                    self.image_label.setPixmap(pixmap)
        except Exception as e:
            print(f"更新图像显示失败: {e}")
    
    def update_image(self):
        """更新图像显示"""
        if not self.refresh_checkbox.isChecked():
            return
        
        try:
            if self.main_window:
                if self.is_left_camera and self.main_window.current_left_image is not None:
                    # 更新左眼图像
                    self.update_image_display(self.main_window.current_left_image)
                elif not self.is_left_camera and self.main_window.current_right_image is not None:
                    # 更新右眼图像
                    self.update_image_display(self.main_window.current_right_image)
        except Exception as e:
            print(f"更新图像失败: {e}")
    
    def force_update_image(self):
        """强制更新图像显示（用于窗口状态变化时）"""
        try:
            if self.main_window:
                if self.is_left_camera and self.main_window.current_left_image is not None:
                    self.update_image_display(self.main_window.current_left_image)
                elif not self.is_left_camera and self.main_window.current_right_image is not None:
                    self.update_image_display(self.main_window.current_right_image)
        except Exception as e:
            print(f"强制更新图像失败: {e}")
    
    def resizeEvent(self, event):
        """窗口大小变化事件"""
        super().resizeEvent(event)
        # 延迟更新图像显示，确保窗口大小已经调整完成
        QTimer.singleShot(100, self.force_update_image)
    
    def closeEvent(self, event):
        """关闭事件"""
        self.refresh_timer.stop()
        event.accept()


class ClickableImageLabel(QLabel):
    """可点击的图像标签"""
    
    def __init__(self, text="", parent=None):
        super().__init__(text, parent)
        self.setCursor(Qt.PointingHandCursor)  # 鼠标悬停时显示手型光标
        self.original_image = None
        self.image_title = ""
    
    def setOriginalImage(self, image, title=""):
        """设置原始图像数据"""
        self.original_image = image
        self.image_title = title
    
    def mouseDoubleClickEvent(self, event: QMouseEvent):
        """双击事件处理"""
        if self.original_image is not None:
            # 从父窗口获取相机线程
            camera_thread = None
            parent = self.parent()
            while parent:
                if hasattr(parent, 'camera_thread'):
                    camera_thread = parent.camera_thread
                    break
                parent = parent.parent()
            
            dialog = FullSizeImageDialog(self.original_image, self.image_title, self.parent(), camera_thread)
            dialog.show()


class ZEDExposureControlGUI(QMainWindow):
    """ZED相机曝光控制GUI界面"""
    
    def __init__(self):
        super().__init__()
        self.zed = None
        self.camera_thread = None
        self.coordinate_thread = None  # 三维坐标计算线程
        
        # 获取当前程序文件所在目录
        current_dir = os.path.dirname(os.path.abspath(__file__))
        self.img_folder = os.path.join(current_dir, "img")  # 图片保存文件夹
        self.config_file = os.path.join(current_dir, "zed_camera_config.json")  # 配置文件
        
        self.img_counter = 1     # 图片编号计数器
        self.current_left_image = None   # 当前左眼图像
        self.current_right_image = None  # 当前右眼图像
        self.full_size_dialogs = []  # 保存打开的全尺寸对话框
        
        # 光斑检测结果
        self.left_centroids = []
        self.right_centroids = []
        self.left_contours = []
        self.right_contours = []
        
        self.init_ui()
        self.load_config()  # 加载配置
        
        # 初始化滑动平均处理器
        self.coordinate_smoother = CoordinateSmoothingProcessor(window_size=5)
        
        self.init_camera()
        
        # 创建定时器来同步曝光值
        self.exposure_sync_timer = QTimer()
        self.exposure_sync_timer.timeout.connect(self.sync_exposure_value)
        self.exposure_sync_timer.start(2000)  # 每2秒同步一次
    
    def init_ui(self):
        """初始化用户界面"""
        self.setWindowTitle("ZED双目视觉测量系统")
        self.setGeometry(100, 100, 1400, 900)
        
        # 创建中央部件
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        
        # 创建主布局 - 垂直布局
        main_layout = QVBoxLayout(central_widget)
        
        # 上半部分：三维坐标显示区域
        top_coord_layout = QHBoxLayout()
        
        # 三维坐标显示区域（毫米和厘米单位在同一个label中上下堆叠）
        coord_group = QGroupBox("三维坐标测量结果")
        coord_layout = QVBoxLayout(coord_group)
        
        # 创建统一的坐标显示标签
        self.coord_display_label = QLabel("等待计算...")
        self.coord_display_label.setAlignment(Qt.AlignCenter)
        self.coord_display_label.setStyleSheet("""
            QLabel {
                font-size: 36px;
                font-weight: bold;
                color: #1E3A8A;
                background-color: #F0F4FF;
                border: 3px solid #1E3A8A;
                border-radius: 8px;
                padding: 20px;
                min-height: 120px;
                line-height: 1.5;
            }
        """)
        coord_layout.addWidget(self.coord_display_label)
        
        # 将坐标显示区域添加到上半部分，设置拉伸比例为3
        top_coord_layout.addWidget(coord_group, 3)
        
        # 滑动平均控制面板
        smoothing_group = QGroupBox("滑动平均控制")
        smoothing_layout = QVBoxLayout(smoothing_group)
        
        # 滑动平均开关
        self.smoothing_enabled_checkbox = QCheckBox("启用滑动平均")
        self.smoothing_enabled_checkbox.setChecked(True)
        self.smoothing_enabled_checkbox.toggled.connect(self.toggle_smoothing)
        smoothing_layout.addWidget(self.smoothing_enabled_checkbox)
        
        # 滑动平均次数设置
        smoothing_count_layout = QHBoxLayout()
        smoothing_count_layout.addWidget(QLabel("平均次数:"))
        self.smoothing_count_spinbox = QSpinBox()
        self.smoothing_count_spinbox.setMinimum(2)
        self.smoothing_count_spinbox.setMaximum(20)
        self.smoothing_count_spinbox.setValue(5)
        self.smoothing_count_spinbox.valueChanged.connect(self.update_smoothing_params)
        smoothing_count_layout.addWidget(self.smoothing_count_spinbox)
        smoothing_layout.addLayout(smoothing_count_layout)
        
        # 当前状态显示
        self.smoothing_status_label = QLabel("状态: 已启用 (5次平均)")
        self.smoothing_status_label.setAlignment(Qt.AlignCenter)
        self.smoothing_status_label.setStyleSheet("font-size: 12px; color: #666; padding: 5px; background-color: #f0f0f0; border: 1px solid #ccc; border-radius: 4px;")
        smoothing_layout.addWidget(self.smoothing_status_label)
        
        # 重置按钮
        self.reset_smoothing_btn = QPushButton("重置滑动平均")
        self.reset_smoothing_btn.setMaximumWidth(120)
        self.reset_smoothing_btn.clicked.connect(self.reset_smoothing)
        self.reset_smoothing_btn.setStyleSheet("QPushButton { background-color: #FF9800; color: white; font-weight: bold; padding: 5px; border-radius: 4px; }")
        smoothing_layout.addWidget(self.reset_smoothing_btn)
        
        # 添加弹性空间
        smoothing_layout.addStretch()
        
        # 将滑动平均控制面板添加到上半部分，设置拉伸比例为1
        top_coord_layout.addWidget(smoothing_group, 1)
        
        # 将上半部分添加到主布局
        main_layout.addLayout(top_coord_layout)
        
        # 中间部分：图像显示区域和控制面板
        middle_layout = QHBoxLayout()
        
        # 图像显示区域 - 左右排布
        image_layout = QHBoxLayout()
        
        # 左眼图像
        left_group = QGroupBox("左眼相机")
        left_layout = QVBoxLayout(left_group)
        self.left_image_label = ClickableImageLabel("等待图像...")
        self.left_image_label.setMinimumSize(640, 480)
        self.left_image_label.setAlignment(Qt.AlignCenter)
        self.left_image_label.setStyleSheet("border: 2px solid gray;")
        left_layout.addWidget(self.left_image_label)
        image_layout.addWidget(left_group)
        
        # 右眼图像
        right_group = QGroupBox("右眼相机")
        right_layout = QVBoxLayout(right_group)
        self.right_image_label = ClickableImageLabel("等待图像...")
        self.right_image_label.setMinimumSize(640, 480)
        self.right_image_label.setAlignment(Qt.AlignCenter)
        self.right_image_label.setStyleSheet("border: 2px solid gray;")
        right_layout.addWidget(self.right_image_label)
        image_layout.addWidget(right_group)
        
        middle_layout.addLayout(image_layout)
        
        # 右侧控制面板
        control_widget = QWidget()
        control_widget.setMaximumWidth(300)  # 限制控制面板宽度
        control_layout = QVBoxLayout(control_widget)
        
        # 相机设置组
        camera_group = QGroupBox("相机设置")
        camera_layout = QVBoxLayout(camera_group)
        
        # 分辨率设置
        resolution_layout = QHBoxLayout()
        resolution_layout.addWidget(QLabel("分辨率:"))
        self.resolution_combo = QComboBox()
        self.resolution_combo.addItems(["HD 2K", "HD 1080", "HD 720", "VGA"])
        self.resolution_combo.setCurrentText("HD 720")  # 默认选择HD 720
        self.resolution_combo.currentTextChanged.connect(self.on_resolution_changed)
        resolution_layout.addWidget(self.resolution_combo)
        camera_layout.addLayout(resolution_layout)
        
        # 当前分辨率显示
        self.current_resolution_label = QLabel("当前分辨率: HD 720")
        self.current_resolution_label.setAlignment(Qt.AlignCenter)
        self.current_resolution_label.setStyleSheet("font-size: 12px; color: #666; padding: 3px;")
        camera_layout.addWidget(self.current_resolution_label)
        
        # 自动聚焦设置
        focus_layout = QHBoxLayout()
        focus_layout.addWidget(QLabel("自动聚焦:"))
        self.auto_focus_checkbox = QCheckBox("启用")
        self.auto_focus_checkbox.setChecked(True)  # 默认启用自动聚焦
        self.auto_focus_checkbox.toggled.connect(self.toggle_auto_focus)
        focus_layout.addWidget(self.auto_focus_checkbox)
        camera_layout.addLayout(focus_layout)
        
        # 手动聚焦按钮
        self.manual_focus_btn = QPushButton("手动聚焦")
        self.manual_focus_btn.clicked.connect(self.trigger_manual_focus)
        self.manual_focus_btn.setStyleSheet("QPushButton { background-color: #2196F3; color: white; font-weight: bold; padding: 5px; }")
        camera_layout.addWidget(self.manual_focus_btn)
        
        control_layout.addWidget(camera_group)
        
        # 曝光控制组
        exposure_group = QGroupBox("曝光控制")
        exposure_layout = QVBoxLayout(exposure_group)
        
        # 曝光值显示
        self.exposure_label = QLabel("当前曝光: 0")
        self.exposure_label.setAlignment(Qt.AlignCenter)
        self.exposure_label.setStyleSheet("font-size: 14px; font-weight: bold;")
        exposure_layout.addWidget(self.exposure_label)
        
        # 曝光滑块
        self.exposure_slider = QSlider(Qt.Horizontal)
        self.exposure_slider.setMinimum(0)
        self.exposure_slider.setMaximum(100)
        self.exposure_slider.setValue(50)
        self.exposure_slider.setTickPosition(QSlider.TicksBelow)
        self.exposure_slider.setTickInterval(10)
        self.exposure_slider.valueChanged.connect(self.on_exposure_slider_changed)
        exposure_layout.addWidget(self.exposure_slider)
        
        # 快速调节按钮
        button_layout = QHBoxLayout()
        
        self.decrease_btn = QPushButton("减少曝光")
        self.decrease_btn.clicked.connect(self.decrease_exposure)
        button_layout.addWidget(self.decrease_btn)
        
        self.increase_btn = QPushButton("增加曝光")
        self.increase_btn.clicked.connect(self.increase_exposure)
        button_layout.addWidget(self.increase_btn)
        
        exposure_layout.addLayout(button_layout)
        
        # 重置按钮
        self.reset_btn = QPushButton("重置曝光")
        self.reset_btn.clicked.connect(self.reset_exposure)
        exposure_layout.addWidget(self.reset_btn)
        
        control_layout.addWidget(exposure_group)
        
        # 光斑检测控制组
        blob_group = QGroupBox("光斑检测")
        blob_layout = QVBoxLayout(blob_group)
        
        self.blob_enable_checkbox = QCheckBox("启用光斑检测")
        self.blob_enable_checkbox.setChecked(True)
        self.blob_enable_checkbox.toggled.connect(self.toggle_blob_detection)
        blob_layout.addWidget(self.blob_enable_checkbox)
        
        # 检测参数调节
        param_layout = QHBoxLayout()
        param_layout.addWidget(QLabel("最小面积:"))
        self.min_area_spinbox = QSpinBox()
        self.min_area_spinbox.setMinimum(1)
        self.min_area_spinbox.setMaximum(1000)
        self.min_area_spinbox.setValue(1)
        self.min_area_spinbox.valueChanged.connect(self.update_blob_detector_params)
        param_layout.addWidget(self.min_area_spinbox)
        blob_layout.addLayout(param_layout)
        
        param_layout2 = QHBoxLayout()
        param_layout2.addWidget(QLabel("最大面积:"))
        self.max_area_spinbox = QSpinBox()
        self.max_area_spinbox.setMinimum(100)
        self.max_area_spinbox.setMaximum(50000)
        self.max_area_spinbox.setValue(10000)
        self.max_area_spinbox.valueChanged.connect(self.update_blob_detector_params)
        param_layout2.addWidget(self.max_area_spinbox)
        blob_layout.addLayout(param_layout2)
        
        # 二值化阈值调节
        threshold_layout = QHBoxLayout()
        threshold_layout.addWidget(QLabel("二值化阈值:"))
        self.threshold_spinbox = QSpinBox()
        self.threshold_spinbox.setMinimum(50)
        self.threshold_spinbox.setMaximum(255)
        self.threshold_spinbox.setValue(150)
        self.threshold_spinbox.valueChanged.connect(self.update_blob_detector_params)
        threshold_layout.addWidget(self.threshold_spinbox)
        blob_layout.addLayout(threshold_layout)
        
        # 光斑稳定性参数调节
        stability_layout = QHBoxLayout()
        stability_layout.addWidget(QLabel("位置容差:"))
        self.position_threshold_spinbox = QSpinBox()
        self.position_threshold_spinbox.setMinimum(5)
        self.position_threshold_spinbox.setMaximum(30)
        self.position_threshold_spinbox.setValue(10)
        self.position_threshold_spinbox.valueChanged.connect(self.update_blob_detector_params)
        stability_layout.addWidget(self.position_threshold_spinbox)
        blob_layout.addLayout(stability_layout)
        
        stability_layout2 = QHBoxLayout()
        stability_layout2.addWidget(QLabel("稳定性阈值:"))
        self.stability_threshold_spinbox = QSpinBox()
        self.stability_threshold_spinbox.setMinimum(2)
        self.stability_threshold_spinbox.setMaximum(10)
        self.stability_threshold_spinbox.setValue(3)
        self.stability_threshold_spinbox.valueChanged.connect(self.update_blob_detector_params)
        stability_layout2.addWidget(self.stability_threshold_spinbox)
        blob_layout.addLayout(stability_layout2)
        
        control_layout.addWidget(blob_group)
        
        # 图片保存组
        save_group = QGroupBox("图片保存")
        save_layout = QVBoxLayout(save_group)
        
        self.save_btn = QPushButton("保存当前图片")
        self.save_btn.clicked.connect(self.save_current_images)
        self.save_btn.setStyleSheet("QPushButton { background-color: #4CAF50; color: white; font-weight: bold; padding: 8px; }")
        save_layout.addWidget(self.save_btn)
        
        self.save_info_label = QLabel("图片和配置将保存到 img/ 文件夹")
        self.save_info_label.setAlignment(Qt.AlignCenter)
        self.save_info_label.setStyleSheet("color: gray; font-size: 10px; padding: 3px;")
        save_layout.addWidget(self.save_info_label)
        
        control_layout.addWidget(save_group)
        
        # 添加弹性空间
        control_layout.addStretch()
        
        middle_layout.addWidget(control_widget)
        
        # 将中间部分添加到主布局
        main_layout.addLayout(middle_layout)
        
        # 下半部分：实时曲线图显示区域
        bottom_layout = QHBoxLayout()
        
        # 实时曲线图显示区域
        if MATPLOTLIB_AVAILABLE:
            plot_group = QGroupBox("Real-time Coordinate Curves")
            plot_layout = QVBoxLayout(plot_group)
            
            # 创建matplotlib图表
            self.figure = Figure(figsize=(12, 6))
            self.canvas = FigureCanvas(self.figure)
            plot_layout.addWidget(self.canvas)
            
            # 创建三个子图
            self.ax1 = self.figure.add_subplot(131)  # X轴坐标
            self.ax2 = self.figure.add_subplot(132)  # Y轴坐标
            self.ax3 = self.figure.add_subplot(133)  # Z轴坐标
            
            # 设置子图标题和标签（英文显示，单位改为厘米）
            self.ax1.set_title('X Coordinate (cm)', fontsize=12)
            self.ax1.set_xlabel('Time (s)')
            self.ax1.set_ylabel('Coordinate (cm)')
            self.ax1.grid(True, alpha=0.3)
            
            self.ax2.set_title('Y Coordinate (cm)', fontsize=12)
            self.ax2.set_xlabel('Time (s)')
            self.ax2.set_ylabel('Coordinate (cm)')
            self.ax2.grid(True, alpha=0.3)
            
            self.ax3.set_title('Z Coordinate (cm)', fontsize=12)
            self.ax3.set_xlabel('Time (s)')
            self.ax3.set_ylabel('Coordinate (cm)')
            self.ax3.grid(True, alpha=0.3)
            
            # 调整子图间距
            self.figure.tight_layout()
            
            bottom_layout.addWidget(plot_group)
        else:
            # 如果没有matplotlib，显示提示信息
            no_plot_label = QLabel("实时曲线图需要安装matplotlib库")
            no_plot_label.setAlignment(Qt.AlignCenter)
            no_plot_label.setStyleSheet("font-size: 16px; color: #666; padding: 20px;")
            bottom_layout.addWidget(no_plot_label)
        
        # 将下半部分添加到主布局
        main_layout.addLayout(bottom_layout)
        
        # 设置布局比例
        main_layout.setStretch(0, 1)  # 上半部分（三维坐标）占1份
        main_layout.setStretch(1, 3)  # 中间部分（图像）占3份
        main_layout.setStretch(2, 2)  # 下半部分（曲线图）占2份
        
        # 初始化坐标显示标签
        self.init_coordinate_labels()
    
    def init_coordinate_labels(self):
        """初始化坐标显示标签"""
        try:
            # 设置统一坐标显示标签的初始状态
            self.coord_display_label.setText("等待检测光斑...")
            self.coord_display_label.setStyleSheet("""
                QLabel {
                    font-size: 36px;
                    font-weight: bold;
                    color: #1E3A8A;
                    background-color: #F0F4FF;
                    border: 3px solid #1E3A8A;
                    border-radius: 8px;
                    padding: 20px;
                    min-height: 120px;
                    line-height: 1.5;
                }
            """)
            
            print("✓ 坐标显示标签初始化完成")
            
        except Exception as e:
            print(f"初始化坐标显示标签失败: {e}")
    
    def format_coordinate_text(self, x, y, z, unit="mm"):
        """格式化坐标显示文本"""
        try:
            if unit.lower() == "mm":
                return f"X: {x:.2f} mm    Y: {y:.2f} mm    Z: {z:.2f} mm"
            elif unit.lower() == "cm":
                return f"X: {x:.2f} cm    Y: {y:.2f} cm    Z: {z:.2f} cm"
            else:
                return f"X: {x:.2f} {unit}    Y: {y:.2f} {unit}    Z: {z:.2f} {unit}"
        except Exception as e:
            print(f"格式化坐标文本失败: {e}")
            return "坐标格式化错误"
    
    def init_camera(self):
        """初始化相机"""
        try:
            # 检查ZED SDK版本
            print(f"ZED SDK版本: {sl.Camera.get_sdk_version()}")
            
            # 检查可用相机数量
            available_cameras = sl.Camera.get_device_list()
            print(f"检测到 {len(available_cameras)} 个ZED相机")
            
            if len(available_cameras) == 0:
                QMessageBox.critical(self, "错误", "未检测到ZED相机，请检查USB连接")
                return
            
            # 显示相机信息
            for i, camera in enumerate(available_cameras):
                print(f"相机 {i+1}: {camera.camera_model} (SN: {camera.serial_number})")
            
            # 创建相机对象
            self.zed = sl.Camera()
            
            # 设置初始化参数 - 使用界面选择的分辨率
            init_params = sl.InitParameters()
            
            # 根据界面选择设置分辨率
            resolution_map = {
                "HD 2K": sl.RESOLUTION.HD2K,
                "HD 1080": sl.RESOLUTION.HD1080,
                "HD 720": sl.RESOLUTION.HD720,
                "VGA": sl.RESOLUTION.VGA
            }
            
            current_resolution = self.resolution_combo.currentText()
            if current_resolution in resolution_map:
                init_params.camera_resolution = resolution_map[current_resolution]
                print(f"✓ 设置相机分辨率: {current_resolution}")
            else:
                init_params.camera_resolution = sl.RESOLUTION.VGA
                print(f"⚠ 使用默认分辨率: VGA")
            
            # 打开相机（带重试机制）
            max_retries = 3
            for attempt in range(max_retries):
                try:
                    err = self.zed.open(init_params)
                    if err == sl.ERROR_CODE.SUCCESS:
                        break
                    else:
                        print(f"尝试 {attempt + 1}/{max_retries} 失败，错误代码: {err}")
                        if attempt < max_retries - 1:
                            print("等待2秒后重试...")
                            time.sleep(2)
                        else:
                            # 最后一次尝试失败，尝试使用默认参数
                            print("尝试使用默认参数重新初始化...")
                            try:
                                default_params = sl.InitParameters()
                                err = self.zed.open(default_params)
                                if err == sl.ERROR_CODE.SUCCESS:
                                    print("✓ 使用默认参数初始化成功")
                                    break
                                else:
                                    error_msg = f"无法打开 ZED 相机，错误代码: {err}"
                                    print(f"✗ {error_msg}")
                                    QMessageBox.critical(self, "错误", error_msg)
                                    return
                            except Exception as default_e:
                                error_msg = f"相机初始化失败: {default_e}"
                                print(f"✗ {error_msg}")
                                QMessageBox.critical(self, "错误", error_msg)
                                return
                except Exception as e:
                    print(f"尝试 {attempt + 1}/{max_retries} 出现异常: {e}")
                    if attempt < max_retries - 1:
                        print("等待2秒后重试...")
                        time.sleep(2)
                    else:
                        error_msg = f"相机初始化异常: {e}"
                        print(f"✗ {error_msg}")
                        QMessageBox.critical(self, "错误", error_msg)
                        return
            
            # 安全地获取相机信息
            try:
                camera_info = self.zed.get_camera_information()
                print(f"✓ 相机型号: {camera_info.camera_model}")
                print(f"✓ 相机序列号: {camera_info.serial_number}")
                
                # 安全地获取分辨率信息
                try:
                    real_width = camera_info.camera_configuration.resolution.width
                    real_height = camera_info.camera_configuration.resolution.height
                    print(f"✓ 真实分辨率: {real_width} x {real_height}")
                    
                    # 更新界面显示的真实分辨率
                    self.current_resolution_label.setText(f"当前分辨率: {real_width} × {real_height}")
                except:
                    print("✓ 分辨率: 信息不可用")
                
                # 安全地获取帧率信息
                try:
                    print(f"✓ 帧率: {camera_info.camera_configuration.fps} FPS")
                except:
                    print("✓ 帧率: 信息不可用")
                
                # 安全地获取传感器分辨率（如果存在）
                try:
                    if hasattr(camera_info.camera_configuration, 'sensor_resolution'):
                        print(f"✓ 传感器分辨率: {camera_info.camera_configuration.sensor_resolution.width} x {camera_info.camera_configuration.sensor_resolution.height}")
                    else:
                        print("✓ 传感器分辨率: 信息不可用")
                except:
                    print("✓ 传感器分辨率: 信息不可用")
            except Exception as e:
                print(f"⚠ 获取相机信息失败: {e}")
                print("✓ 相机初始化成功（但无法获取详细信息）")
            
            print("✓ 相机初始化成功")
            print("  光斑检测: 已启用")
            
            # 创建并启动相机线程
            self.camera_thread = ZEDCameraThread(self.zed, self)
            self.camera_thread.image_processed_ready.connect(self.on_image_processed)
            self.camera_thread.error_occurred.connect(self.handle_error)
            
            # 启动线程
            self.camera_thread.start()
            
            # 创建并启动图像显示线程
            self.image_display_thread = ImageDisplayThread()
            self.image_display_thread.left_image_display_ready.connect(self.update_left_image)
            self.image_display_thread.right_image_display_ready.connect(self.update_right_image)
            self.image_display_thread.blob_detection_ready.connect(self.on_blob_detection_ready)
            
            # 启动图像显示线程
            self.image_display_thread.start()
            
            # 创建并启动三维坐标计算线程
            self.coordinate_thread = ThreeDCoordinateThread()
            self.coordinate_thread.coordinate_ready.connect(self.update_coordinate_display)
            self.coordinate_thread.start()
            
            # 创建并启动数据采集线程
            self.data_collection_thread = DataCollectionThread()
            self.data_collection_thread.data_collected.connect(self.on_coordinate_data_collected)
            self.data_collection_thread.start()
            print("✓ 数据采集线程已启动")
            
            # 创建并启动实时曲线图线程
            if MATPLOTLIB_AVAILABLE:
                self.plot_thread = RealTimePlotThread()
                self.plot_thread.plot_update_ready.connect(self.update_real_time_plots)
                self.plot_thread.start()
                print("✓ 实时曲线图线程已启动")
            
            # 应用保存的曝光设置
            self.apply_saved_exposure()
            
            # 更新滑动平均状态显示
            self.update_smoothing_status()
            
            # 测试相机是否正常工作
            print("正在测试相机图像捕获...")
            test_image = sl.Mat()
            runtime_params = sl.RuntimeParameters()
            
            # 尝试捕获一帧图像
            if self.zed.grab(runtime_params) == sl.ERROR_CODE.SUCCESS:
                self.zed.retrieve_image(test_image, sl.VIEW.LEFT)
                print(f"✓ 相机测试成功，图像尺寸: {test_image.get_width()} x {test_image.get_height()}")
            else:
                print("⚠ 相机测试失败，但继续运行...")
                
        except Exception as e:
            QMessageBox.critical(self, "错误", f"初始化相机失败: {e}")
    
    def apply_saved_exposure(self):
        """应用保存的曝光设置"""
        try:
            if hasattr(self, 'camera_thread') and self.camera_thread:
                # 获取当前滑块的值
                saved_exposure = self.exposure_slider.value()
                print(f"正在应用保存的曝光设置: {saved_exposure}")
                
                # 应用曝光设置
                success = self.camera_thread.set_exposure(saved_exposure)
                if success:
                    print(f"✓ 曝光设置应用成功: {saved_exposure}")
                    # 更新显示
                    self.update_exposure_display(saved_exposure)
                else:
                    print(f"⚠ 曝光设置应用失败，尝试获取当前真实曝光值")
                    # 尝试获取当前真实曝光值
                    real_exposure = self.camera_thread.get_current_exposure()
                    if real_exposure != saved_exposure:
                        print(f"真实曝光值: {real_exposure}，与保存值不同: {saved_exposure}")
                        # 更新滑块和显示为真实值
                        self.exposure_slider.blockSignals(True)
                        self.exposure_slider.setValue(real_exposure)
                        self.exposure_slider.blockSignals(False)
                        self.update_exposure_display(real_exposure)
        except Exception as e:
            print(f"⚠ 应用曝光设置时出错: {e}")
    
    def sync_exposure_value(self):
        """同步真实曝光值"""
        try:
            if hasattr(self, 'camera_thread') and self.camera_thread:
                # 获取真实曝光值
                real_exposure = self.camera_thread.get_current_exposure()
                current_slider_value = self.exposure_slider.value()
                
                # 如果真实值与滑块值不同，更新显示
                if real_exposure != current_slider_value:
                    print(f"曝光值不同步 - 滑块: {current_slider_value}, 真实: {real_exposure}")
                    # 更新滑块值（不触发信号）
                    self.exposure_slider.blockSignals(True)
                    self.exposure_slider.setValue(real_exposure)
                    self.exposure_slider.blockSignals(False)
                    # 更新显示
                    self.update_exposure_display(real_exposure)
        except Exception as e:
            print(f"同步曝光值失败: {e}")
    
    def update_left_image(self, image):
        """更新左眼图像显示"""
        try:
            # 检查图像数据有效性
            if image is None:
                return
                
            if len(image.shape) != 3 or image.shape[2] != 3:
                return
            
            # 保存当前图像用于保存功能
            self.current_left_image = image.copy()
            
            # 保存原始图像数据到可点击标签
            self.left_image_label.setOriginalImage(image, "左眼相机 - 原尺寸图像")
            
            # 转换为QImage (BGR格式)
            height, width, channel = image.shape
            bytes_per_line = 3 * width
            
            # 确保图像数据类型正确
            if image.dtype != np.uint8:
                image = image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            
            # 根据标签大小缩放图像，保持宽高比
            scaled_pixmap = pixmap.scaled(
                self.left_image_label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            )
            self.left_image_label.setPixmap(scaled_pixmap)
            
            # 更新分辨率显示
            self.current_resolution_label.setText(f"当前分辨率: {width} × {height}")
            
        except Exception as e:
            print(f"更新左眼图像失败: {e}")
    
    def update_right_image(self, image):
        """更新右眼图像显示"""
        try:
            # 检查图像数据有效性
            if image is None:
                return
                
            if len(image.shape) != 3 or image.shape[2] != 3:
                return
            
            # 保存当前图像用于保存功能
            self.current_right_image = image.copy()
            
            # 保存原始图像数据到可点击标签
            self.right_image_label.setOriginalImage(image, "右眼相机 - 原尺寸图像")
            
            # 转换为QImage (BGR格式)
            height, width, channel = image.shape
            bytes_per_line = 3 * width
            
            # 确保图像数据类型正确
            if image.dtype != np.uint8:
                image = image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            
            # 根据标签大小缩放图像，保持宽高比
            scaled_pixmap = pixmap.scaled(
                self.right_image_label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            )
            self.right_image_label.setPixmap(scaled_pixmap)
            
        except Exception as e:
            print(f"更新右眼图像失败: {e}")
    

    

    


    

    

    

    
    def save_current_images(self):
        """保存当前左右相机图片"""
        try:
            # 检查是否有图像数据
            if self.current_left_image is None or self.current_right_image is None:
                QMessageBox.warning(self, "警告", "没有可保存的图像数据")
                return
            
            # 创建保存文件夹
            if not os.path.exists(self.img_folder):
                os.makedirs(self.img_folder)
            
            # 生成文件名
            left_filename = os.path.join(self.img_folder, f"left{self.img_counter}.jpg")
            right_filename = os.path.join(self.img_folder, f"right{self.img_counter}.jpg")
            
            # 保存图片
            cv2.imwrite(left_filename, self.current_left_image)
            cv2.imwrite(right_filename, self.current_right_image)
            

            
            # 更新计数器
            self.img_counter += 1
            
            # 显示成功消息
            QMessageBox.information(self, "保存成功", 
                                  f"图片已保存到 {self.img_folder}/ 文件夹\n"
                                  f"左眼: left{self.img_counter-1}.jpg\n"
                                  f"右眼: right{self.img_counter-1}.jpg")
            
        except Exception as e:
            error_msg = f"保存图片失败: {e}"
            QMessageBox.critical(self, "保存失败", error_msg)
    

    

    

    

    

            # 更新左眼图像显示
            if left_image is not None:
                self.update_image_display(left_image, self.left_image_label)
            
            # 更新右眼图像显示
            if right_image is not None:
                self.update_image_display(right_image, self.right_image_label)
            
            # 处理三维坐标数据
            if isinstance(result_data, dict):
                simple_result = result_data.get('simple')
                if simple_result is not None:
                    print(f"✓ 图像已更新，简单算法三维坐标: X={simple_result[0]:.1f}mm, Y={simple_result[1]:.1f}mm, Z={simple_result[2]:.1f}mm")
            else:
                print(f"✓ 图像已更新，三维坐标: X={result_data[0]:.1f}mm, Y={result_data[1]:.1f}mm, Z={result_data[2]:.1f}mm")
            
        except Exception as e:
            print(f"更新图像显示失败: {e}")
            import traceback
            traceback.print_exc()
    
    def update_image_display(self, image, label):
        """更新图像显示"""
        try:
            # 检查图像数据有效性
            if image is None:
                return
                
            if len(image.shape) != 3 or image.shape[2] != 3:
                return
            
            # 确保图像数据类型正确
            if image.dtype != np.uint8:
                image = image.astype(np.uint8)
            
            # 将BGR转换为RGB格式用于QImage显示
            rgb_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            
            # 转换为QImage
            height, width, channel = rgb_image.shape
            bytes_per_line = 3 * width
            q_image = QImage(rgb_image.data, width, height, bytes_per_line, QImage.Format_RGB888)
            
            # 转换为QPixmap并显示
            pixmap = QPixmap.fromImage(q_image)
            label.setPixmap(pixmap.scaled(
                label.size(), 
                Qt.KeepAspectRatio, 
                Qt.SmoothTransformation
            ))
            
        except Exception as e:
            print(f"更新图像显示失败: {e}")
    
    def on_image_processed(self, left_image, right_image, left_centroids, right_centroids, left_contours, right_contours):
        """处理从相机线程接收到的图像和光斑检测结果"""
        try:
            # 将图像数据发送到图像显示线程
            if hasattr(self, 'image_display_thread'):
                self.image_display_thread.add_image_data(
                    left_image, right_image,
                    left_centroids, right_centroids,
                    left_contours, right_contours
                )
                
        except Exception as e:
            print(f"处理图像数据失败: {e}")
    
    def on_blob_detection_ready(self, left_centroids, right_centroids, left_contours, right_contours):
        """处理光斑检测结果"""
        try:
            # 保存光斑检测结果
            self.left_centroids = left_centroids
            self.right_centroids = right_centroids
            self.left_contours = left_contours
            self.right_contours = right_contours
            
            # 发送到三维坐标计算线程
            if self.coordinate_thread:
                self.coordinate_thread.update_centroids(left_centroids, right_centroids)
                self.coordinate_thread.update_contours(left_contours, right_contours)
                
        except Exception as e:
            print(f"处理光斑检测结果失败: {e}")
    
    def update_coordinate_display(self, result_data):
        """更新三维坐标显示（毫米和厘米单位在同一个label中上下堆叠）"""
        try:
            # 检查结果数据类型
            if isinstance(result_data, dict):
                # 只显示OpenCV算法结果
                opencv_result = result_data.get('opencv')
                
                # 检查是否有光斑检测结果
                has_blobs = (len(self.left_centroids) > 0 or len(self.right_centroids) > 0)
                
                # 更新OpenCV算法结果
                if opencv_result is not None:
                    # 应用滑动平均处理
                    if hasattr(self, 'coordinate_smoother'):
                        smoothed_x, smoothed_y, smoothed_z = self.coordinate_smoother.add_coordinate(
                            opencv_result[0], opencv_result[1], opencv_result[2]
                        )
                    else:
                        smoothed_x, smoothed_y, smoothed_z = opencv_result[0], opencv_result[1], opencv_result[2]
                    
                    # 毫米单位显示（使用平滑后的数据）
                    mm_text = self.format_coordinate_text(smoothed_x, smoothed_y, smoothed_z, "mm")
                    
                    # 厘米单位显示（使用平滑后的数据）
                    cm_x = smoothed_x / 10.0
                    cm_y = smoothed_y / 10.0
                    cm_z = smoothed_z / 10.0
                    cm_text = self.format_coordinate_text(cm_x, cm_y, cm_z, "cm")
                    
                    # 在同一个label中上下堆叠显示
                    combined_text = f"{mm_text}\n\n{cm_text}"
                    self.coord_display_label.setText(combined_text)
                    
                    # 将数据发送到数据采集线程（轻量级操作，转换为厘米单位）
                    if hasattr(self, 'data_collection_thread'):
                        # 将毫米转换为厘米（原始数据）
                        x_raw_cm = opencv_result[0] / 10.0
                        y_raw_cm = opencv_result[1] / 10.0
                        z_raw_cm = opencv_result[2] / 10.0
                        # 将毫米转换为厘米（平滑后数据）
                        x_smooth_cm = smoothed_x / 10.0
                        y_smooth_cm = smoothed_y / 10.0
                        z_smooth_cm = smoothed_z / 10.0
                        self.data_collection_thread.add_coordinate_data(x_raw_cm, y_raw_cm, z_raw_cm, x_smooth_cm, y_smooth_cm, z_smooth_cm)
                    
                    print(f"✓ 三维坐标显示 - 毫米: {mm_text}")
                    print(f"✓ 三维坐标显示 - 厘米: {cm_text}")
                elif has_blobs:
                    # 有光斑但没有计算出三维坐标
                    self.coord_display_label.setText("光斑已检测，正在计算三维坐标...")
                    print(f"⚠ 光斑已检测但三维坐标计算失败 - 左: {len(self.left_centroids)} 个, 右: {len(self.right_centroids)} 个")
                else:
                    # 没有光斑
                    self.coord_display_label.setText("等待检测光斑...")
                    print("等待检测光斑...")
                
            else:
                # 兼容旧格式，单个结果
                if hasattr(self, 'coord_display_label'):
                    current_coord_text = f"X: {result_data[0]:.1f} mm (右)\nY: {result_data[1]:.1f} mm (下)\nZ: {result_data[2]:.1f} mm (前)"
                    self.coord_display_label.setText(current_coord_text)
            
        except Exception as e:
            print(f"更新三维坐标显示失败: {e}")
    
    def toggle_smoothing(self, enabled):
        """切换滑动平均开关"""
        try:
            if hasattr(self, 'coordinate_smoother'):
                self.coordinate_smoother.enable(enabled)
                self.update_smoothing_status()
                print(f"✓ 滑动平均已{'启用' if enabled else '禁用'}")
            # 自动保存配置
            self.save_config()
        except Exception as e:
            print(f"切换滑动平均失败: {e}")
    
    def update_smoothing_params(self):
        """更新滑动平均参数"""
        try:
            if hasattr(self, 'coordinate_smoother'):
                new_count = self.smoothing_count_spinbox.value()
                self.coordinate_smoother.set_window_size(new_count)
                self.update_smoothing_status()
                print(f"✓ 滑动平均次数已更新为: {new_count}")
            # 自动保存配置
            self.save_config()
        except Exception as e:
            print(f"更新滑动平均参数失败: {e}")
    
    def reset_smoothing(self):
        """重置滑动平均器"""
        try:
            if hasattr(self, 'coordinate_smoother'):
                self.coordinate_smoother.reset()
                self.update_smoothing_status()
                print("✓ 滑动平均器已重置")
        except Exception as e:
            print(f"重置滑动平均器失败: {e}")
    
    def update_smoothing_status(self):
        """更新滑动平均状态显示"""
        try:
            if hasattr(self, 'coordinate_smoother'):
                status_text = self.coordinate_smoother.get_status_info()
                self.smoothing_status_label.setText(status_text)
        except Exception as e:
            print(f"更新滑动平均状态失败: {e}")
    
    def on_coordinate_data_collected(self, x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth):
        """处理从数据采集线程收集到的坐标数据"""
        try:
            # 将数据转发给实时曲线图线程
            if hasattr(self, 'plot_thread') and self.plot_thread:
                self.plot_thread.add_coordinate_data(x_raw, y_raw, z_raw, x_smooth, y_smooth, z_smooth)
        except Exception as e:
            print(f"处理坐标数据失败: {e}")
    
    def update_real_time_plots(self, plot_data):
        """更新实时曲线图（显示原始数据和平滑数据）"""
        try:
            if not MATPLOTLIB_AVAILABLE or not hasattr(self, 'ax1'):
                return
            
            # 只在有数据时才更新图表
            if len(plot_data['timestamps']) == 0:
                return
            
            # 清除之前的图表
            self.ax1.clear()
            self.ax2.clear()
            self.ax3.clear()
            
            # 设置子图标题和标签（英文显示，单位改为厘米，减少字体大小，提高性能）
            self.ax1.set_title('X Coordinate (cm)', fontsize=10)
            self.ax1.set_xlabel('Time (s)', fontsize=9)
            self.ax1.set_ylabel('Coordinate (cm)', fontsize=9)
            self.ax1.grid(True, alpha=0.2)
            
            self.ax2.set_title('Y Coordinate (cm)', fontsize=10)
            self.ax2.set_xlabel('Time (s)', fontsize=9)
            self.ax2.set_ylabel('Coordinate (cm)', fontsize=9)
            self.ax2.grid(True, alpha=0.2)
            
            self.ax3.set_title('Z Coordinate (cm)', fontsize=10)
            self.ax3.set_xlabel('Time (s)', fontsize=9)
            self.ax3.set_ylabel('Coordinate (cm)', fontsize=9)
            self.ax3.grid(True, alpha=0.2)
            
            # 绘制数据（同时显示原始数据和平滑数据）
            start_time = plot_data['timestamps'][0]
            relative_times = [t - start_time for t in plot_data['timestamps']]
            
            # 绘制原始数据（细线，半透明）
            if 'raw_x_coords' in plot_data and len(plot_data['raw_x_coords']) > 0:
                self.ax1.plot(relative_times, plot_data['raw_x_coords'], 'b-', linewidth=1, alpha=0.4, label='Raw')
                self.ax2.plot(relative_times, plot_data['raw_y_coords'], 'g-', linewidth=1, alpha=0.4, label='Raw')
                self.ax3.plot(relative_times, plot_data['raw_z_coords'], 'r-', linewidth=1, alpha=0.4, label='Raw')
            
            # 绘制平滑数据（粗线，不透明）
            if 'smooth_x_coords' in plot_data and len(plot_data['smooth_x_coords']) > 0:
                self.ax1.plot(relative_times, plot_data['smooth_x_coords'], 'b-', linewidth=2, alpha=0.9, label='Smoothed')
                self.ax2.plot(relative_times, plot_data['smooth_y_coords'], 'g-', linewidth=2, alpha=0.9, label='Smoothed')
                self.ax3.plot(relative_times, plot_data['smooth_z_coords'], 'r-', linewidth=2, alpha=0.9, label='Smoothed')
            
            # 添加图例
            self.ax1.legend(fontsize=8)
            self.ax2.legend(fontsize=8)
            self.ax3.legend(fontsize=8)
            
            # 调整子图间距
            self.figure.tight_layout()
            
            # 刷新画布
            self.canvas.draw()
            
        except Exception as e:
            print(f"更新实时曲线图失败: {e}")
    
    def handle_error(self, error_message):
        """处理错误"""
        pass
    
    def on_exposure_slider_changed(self, value):
        """曝光滑块变化处理"""
        try:
            if self.camera_thread:
                success = self.camera_thread.set_exposure(value)
                if success:
                    # 更新显示
                    self.update_exposure_display(value)
                    print(f"✓ 曝光滑块设置成功: {value}")
                else:
                    print(f"⚠ 曝光滑块设置失败: {value}")
            # 自动保存配置
            self.save_config()
        except Exception as e:
            print(f"曝光滑块变化处理失败: {e}")
    

    
    def increase_exposure(self):
        """增加曝光"""
        if self.camera_thread:
            current_value = self.exposure_slider.value()
            new_value = min(self.exposure_slider.maximum(), current_value + 1)
            self.exposure_slider.setValue(new_value)
    
    def decrease_exposure(self):
        """减少曝光"""
        if self.camera_thread:
            current_value = self.exposure_slider.value()
            new_value = max(self.exposure_slider.minimum(), current_value - 1)
            self.exposure_slider.setValue(new_value)
    
    def reset_exposure(self):
        """重置曝光"""
        if self.camera_thread:
            default_value = (self.exposure_slider.minimum() + self.exposure_slider.maximum()) // 2
            self.exposure_slider.setValue(default_value)
    
    def update_exposure_display(self, exposure_value):
        """更新曝光显示"""
        try:
            # 确保曝光值是整数
            exposure_value = int(exposure_value)
            self.exposure_label.setText(f"当前曝光: {exposure_value}")
            
            # 同时更新滑块的值（如果不同步的话）
            if self.exposure_slider.value() != exposure_value:
                self.exposure_slider.blockSignals(True)  # 阻止信号触发
                self.exposure_slider.setValue(exposure_value)
                self.exposure_slider.blockSignals(False)  # 恢复信号
                
            print(f"✓ 曝光显示已更新: {exposure_value}")
        except Exception as e:
            print(f"更新曝光显示失败: {e}")
    
    def toggle_blob_detection(self, enabled):
        """切换光斑检测开关"""
        if self.camera_thread:
            self.camera_thread.blob_enabled = enabled
        # 自动保存配置
        self.save_config()
    
    def update_blob_detector_params(self):
        """更新光斑检测器参数"""
        if self.camera_thread and self.camera_thread.blob_detector:
            self.camera_thread.blob_detector.min_area = self.min_area_spinbox.value()
            self.camera_thread.blob_detector.max_area = self.max_area_spinbox.value()
            self.camera_thread.blob_detector.threshold_value = self.threshold_spinbox.value()
            
            # 更新稳定性参数
            if hasattr(self, 'position_threshold_spinbox'):
                self.camera_thread.blob_detector.position_threshold = self.position_threshold_spinbox.value()
            if hasattr(self, 'stability_threshold_spinbox'):
                self.camera_thread.blob_detector.stability_threshold = self.stability_threshold_spinbox.value()
            
            print(f"✓ 光斑检测参数已更新 - 最小面积: {self.min_area_spinbox.value()}, 最大面积: {self.max_area_spinbox.value()}, 阈值: {self.threshold_spinbox.value()}")
            print(f"✓ 稳定性参数已更新 - 位置容差: {self.position_threshold_spinbox.value()}, 稳定性阈值: {self.stability_threshold_spinbox.value()}")
            print(f"当前检测器状态 - 面积范围: [{self.camera_thread.blob_detector.min_area}, {self.camera_thread.blob_detector.max_area}], 阈值: {self.camera_thread.blob_detector.threshold_value}")
        # 自动保存配置
        self.save_config()
    
    def toggle_auto_focus(self, enabled):
        """切换自动聚焦开关"""
        if hasattr(self, 'camera_thread') and self.camera_thread:
            success = self.camera_thread.set_auto_focus(enabled)
        # 自动保存配置
        self.save_config()
    
    def trigger_manual_focus(self):
        """触发手动聚焦"""
        if hasattr(self, 'camera_thread') and self.camera_thread:
            # 禁用按钮防止重复点击
            self.manual_focus_btn.setEnabled(False)
            self.manual_focus_btn.setText("聚焦中...")
            
            # 保存当前曝光值
            current_exposure = self.exposure_slider.value()
            print(f"手动聚焦前保存滑块曝光值: {current_exposure}")
            
            # 直接在当前线程中执行聚焦，避免线程管理问题
            try:
                success = self.camera_thread.trigger_manual_focus()
                if success:
                    print("✓ 手动聚焦完成")
                    # 检查曝光值是否被改变
                    new_exposure = self.camera_thread.get_current_exposure()
                    if new_exposure != current_exposure:
                        print(f"曝光值被改变: {current_exposure} -> {new_exposure}，正在恢复...")
                        # 恢复曝光值
                        self.camera_thread.set_exposure(current_exposure)
                        # 更新滑块和显示
                        self.exposure_slider.blockSignals(True)
                        self.exposure_slider.setValue(current_exposure)
                        self.exposure_slider.blockSignals(False)
                        self.update_exposure_display(current_exposure)
                    else:
                        print("✓ 曝光值保持不变")
                else:
                    print("⚠ 手动聚焦失败")
            finally:
                # 恢复按钮状态
                self.manual_focus_btn.setEnabled(True)
                self.manual_focus_btn.setText("手动聚焦")
    
    def on_resolution_changed(self, resolution_text):
        """分辨率设置变化处理"""
        try:
            # 将分辨率文本转换为ZED SDK的枚举值
            resolution_map = {
                "HD 2K": sl.RESOLUTION.HD2K,
                "HD 1080": sl.RESOLUTION.HD1080,
                "HD 720": sl.RESOLUTION.HD720,
                "VGA": sl.RESOLUTION.VGA
            }
            
            if resolution_text in resolution_map:
                new_resolution = resolution_map[resolution_text]
                
                # 更新显示标签
                self.current_resolution_label.setText(f"当前分辨率: {resolution_text}")
                
                # 如果相机已经打开，需要重新初始化
                if hasattr(self, 'zed') and self.zed:
                    self.reinitialize_camera_with_resolution(new_resolution)
                
                # 保存配置
                self.save_config()
        except Exception as e:
            pass
    
    def reinitialize_camera_with_resolution(self, new_resolution):
        """使用新分辨率重新初始化相机"""
        try:
            # 停止当前线程
            if self.camera_thread:
                self.camera_thread.stop()
                self.camera_thread.wait()
            
            # 关闭相机
            if self.zed:
                self.zed.close()
            
            # 重新初始化相机
            init_params = sl.InitParameters()
            init_params.camera_resolution = new_resolution
            
            print(f"正在重新初始化相机，新分辨率: {new_resolution}")
            
            err = self.zed.open(init_params)
            if err != sl.ERROR_CODE.SUCCESS:
                QMessageBox.critical(self, "错误", f"无法使用新分辨率打开相机，错误代码: {err}")
                return
            
            # 获取新的分辨率信息
            try:
                camera_info = self.zed.get_camera_information()
                new_width = camera_info.camera_configuration.resolution.width
                new_height = camera_info.camera_configuration.resolution.height
                print(f"✓ 新分辨率初始化成功: {new_width} x {new_height}")
                
                # 更新界面显示
                resolution_text_map = {
                    sl.RESOLUTION.HD2K: "HD 2K",
                    sl.RESOLUTION.HD1080: "HD 1080", 
                    sl.RESOLUTION.HD720: "HD 720",
                    sl.RESOLUTION.VGA: "VGA"
                }
                if new_resolution in resolution_text_map:
                    self.current_resolution_label.setText(f"当前分辨率: {resolution_text_map[new_resolution]} ({new_width} × {new_height})")
                else:
                    self.current_resolution_label.setText(f"当前分辨率: {new_width} × {new_height}")
            except Exception as e:
                print(f"获取新分辨率信息失败: {e}")
            
            # 重新创建并启动线程
            self.camera_thread = ZEDCameraThread(self.zed, self)
            self.camera_thread.left_image_ready.connect(self.update_left_image)
            self.camera_thread.right_image_ready.connect(self.update_right_image)
            self.camera_thread.error_occurred.connect(self.handle_error)
            self.camera_thread.blob_detection_ready.connect(self.on_blob_detection_ready)
            
            # 启动线程
            self.camera_thread.start()
            
        except Exception as e:
            pass
    

    
    def save_config(self):
        """保存配置文件"""
        try:
            config = {
                'resolution': self.resolution_combo.currentText(),
                'exposure': {
                    'min': self.exposure_slider.minimum(),
                    'max': self.exposure_slider.maximum(),
                    'value': self.exposure_slider.value()
                },
                'blob_detection': {
                    'enabled': self.blob_enable_checkbox.isChecked(),
                    'min_area': self.min_area_spinbox.value(),
                    'max_area': self.max_area_spinbox.value(),
                    'threshold': self.threshold_spinbox.value(),
                    'position_threshold': self.position_threshold_spinbox.value(),
                    'stability_threshold': self.stability_threshold_spinbox.value()
                },
                'focus': {
                    'auto_focus_enabled': self.auto_focus_checkbox.isChecked()
                },
                'coordinate_display': {
                    'show_mm': True,
                    'show_cm': True
                },
                'smoothing': {
                    'enabled': True,
                    'window_size': 5
                },
                'save_info': {
                    'img_counter': self.img_counter,
                    'last_save_time': time.strftime("%Y-%m-%d %H:%M:%S")
                }
            }
            
            config_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "zed_camera_config.json")
            with open(config_file, 'w', encoding='utf-8') as f:
                json.dump(config, f, indent=2, ensure_ascii=False)
            
            print(f"✓ 配置已保存到: {config_file}")
        except Exception as e:
            print(f"✗ 保存配置失败: {e}")
            import traceback
            traceback.print_exc()
    
    def load_config(self):
        """加载配置文件"""
        try:
            config_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "zed_camera_config.json")
            if os.path.exists(config_file):
                print(f"正在加载配置文件: {config_file}")
                with open(config_file, 'r', encoding='utf-8') as f:
                    config = json.load(f)
                
                # 加载分辨率设置
                if 'resolution' in config:
                    self.resolution_combo.setCurrentText(config['resolution'])
                    print(f"✓ 加载分辨率设置: {config['resolution']}")
                else:
                    self.resolution_combo.setCurrentText("HD 720")
                    print("✓ 使用默认分辨率设置: HD 720")
                
                # 加载曝光设置
                if 'exposure' in config:
                    exposure_config = config['exposure']
                    if 'min' in exposure_config:
                        self.exposure_slider.setMinimum(exposure_config['min'])
                    else:
                        self.exposure_slider.setMinimum(0)
                    if 'max' in exposure_config:
                        self.exposure_slider.setMaximum(exposure_config['max'])
                    else:
                        self.exposure_slider.setMaximum(100)
                    if 'value' in exposure_config:
                        self.exposure_slider.setValue(exposure_config['value'])
                        print(f"✓ 加载曝光设置: {exposure_config['value']}")
                    else:
                        self.exposure_slider.setValue(50)
                        print("✓ 使用默认曝光设置: 50")
                
                # 加载光斑检测设置
                if 'blob_detection' in config:
                    blob_config = config['blob_detection']
                    if 'enabled' in blob_config:
                        self.blob_enable_checkbox.setChecked(blob_config['enabled'])
                    else:
                        self.blob_enable_checkbox.setChecked(True)
                    if 'min_area' in blob_config:
                        self.min_area_spinbox.setValue(blob_config['min_area'])
                    else:
                        self.min_area_spinbox.setValue(1)
                    if 'max_area' in blob_config:
                        self.max_area_spinbox.setValue(blob_config['max_area'])
                    else:
                        self.max_area_spinbox.setValue(10000)
                    if 'threshold' in blob_config:
                        self.threshold_spinbox.setValue(blob_config['threshold'])
                    else:
                        self.threshold_spinbox.setValue(150)
                    if 'position_threshold' in blob_config:
                        self.position_threshold_spinbox.setValue(blob_config['position_threshold'])
                    else:
                        self.position_threshold_spinbox.setValue(10)
                    if 'stability_threshold' in blob_config:
                        self.stability_threshold_spinbox.setValue(blob_config['stability_threshold'])
                    else:
                        self.stability_threshold_spinbox.setValue(3)
                    print(f"✓ 加载光斑检测设置: 启用={blob_config.get('enabled', True)}, 面积范围={blob_config.get('min_area', 1)}-{blob_config.get('max_area', 10000)}, 阈值={blob_config.get('threshold', 150)}")
                    print(f"✓ 稳定性参数: 位置容差={blob_config.get('position_threshold', 10)}, 稳定性阈值={blob_config.get('stability_threshold', 3)}")
                
                # 加载聚焦设置
                if 'focus' in config:
                    focus_config = config['focus']
                    if 'auto_focus_enabled' in focus_config:
                        self.auto_focus_checkbox.setChecked(focus_config['auto_focus_enabled'])
                    else:
                        self.auto_focus_checkbox.setChecked(True)
                        print(f"✓ 加载聚焦设置: 自动聚焦={focus_config.get('auto_focus_enabled', True)}")
                
                # 加载滑动平均设置
                if 'smoothing' in config:
                    smoothing_config = config['smoothing']
                    if 'enabled' in smoothing_config:
                        self.smoothing_enabled_checkbox.setChecked(smoothing_config['enabled'])
                    else:
                        self.smoothing_enabled_checkbox.setChecked(True)
                    if 'window_size' in smoothing_config:
                        self.smoothing_count_spinbox.setValue(smoothing_config['window_size'])
                    else:
                        self.smoothing_count_spinbox.setValue(5)
                    print(f"✓ 加载滑动平均设置: 启用={smoothing_config.get('enabled', True)}, 窗口大小={smoothing_config.get('window_size', 5)}")
                
                # 加载保存信息
                if 'save_info' in config:
                    save_config = config['save_info']
                    if 'img_counter' in save_config:
                        self.img_counter = save_config['img_counter']
                        print(f"✓ 加载图片计数器: {self.img_counter}")
                    else:
                        self.img_counter = 1
                        print("✓ 使用默认图片计数器: 1")
                    if 'last_save_time' in save_config:
                        print(f"✓ 上次保存时间: {save_config['last_save_time']}")
                    else:
                        print("✓ 无上次保存时间记录")
                
                print("✓ 配置文件加载完成")
            else:
                print("⚠ 配置文件不存在，将使用默认设置")
        except Exception as e:
            print(f"✗ 加载配置文件失败: {e}")
            import traceback
            traceback.print_exc()
    
    def closeEvent(self, event):
        """窗口关闭事件"""
        try:
            # 停止定时器
            if hasattr(self, 'exposure_sync_timer'):
                self.exposure_sync_timer.stop()
            
            # 保存所有配置到JSON文件
            print("正在保存配置到JSON文件...")
            self.save_config()
            print("✓ 配置保存成功")
        except Exception as e:
            print(f"⚠ 保存配置失败: {e}")
        
        # 停止三维坐标计算线程
        if self.coordinate_thread:
            self.coordinate_thread.stop()
            self.coordinate_thread.wait()
        
        # 停止数据采集线程
        if hasattr(self, 'data_collection_thread') and self.data_collection_thread:
            self.data_collection_thread.stop()
            self.data_collection_thread.wait()
        
        # 停止实时曲线图线程
        if hasattr(self, 'plot_thread') and self.plot_thread:
            self.plot_thread.stop()
            self.plot_thread.wait()
        
        # 停止图像显示线程
        if hasattr(self, 'image_display_thread') and self.image_display_thread:
            self.image_display_thread.stop()
            self.image_display_thread.wait()
        
        # 停止相机线程
        if self.camera_thread:
            self.camera_thread.stop()
            self.camera_thread.wait()
        
        # 关闭相机
        if self.zed:
            self.zed.close()
        
        event.accept()


def main():
    """主函数"""
    
    app = QApplication(sys.argv)
    
    try:
        window = ZEDExposureControlGUI()
        window.show()
        sys.exit(app.exec_())
    except Exception as e:
        QMessageBox.critical(None, "错误", f"程序运行错误: {e}")


if __name__ == "__main__":
    main()
